12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Renamed old classes to "Ordered" and removed "Unordered" name from new classes

release/4.3a0
Richard Roberts 2013-07-29 23:55:40 +00:00
parent 1b92828cc8
commit 3ade190128
320 changed files with 15346 additions and 15346 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
examples/UnorderedLinear.cpp
View File

@ -1,13 +1,13 @@
#include <gtsam/slam/dataset.h> #include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/VariableIndex.h> #include <gtsam/inference/VariableIndexOrdered.h>
#include <gtsam/linear/GaussianFactorGraphUnordered.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValuesUnordered.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h> #include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/linear/GaussianBayesTreeUnordered.h> #include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianJunctionTreeUnordered.h> #include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianEliminationTreeUnordered.h> #include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
@ -129,28 +129,28 @@ ISAM2 solveWithOldISAM2(const NonlinearFactorGraph& measurements)
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraphUnordered convertToUnordered(const GaussianFactorGraph& gfg, const Ordering& ordering) GaussianFactorGraph convertToUnordered(const GaussianFactorGraphOrdered& gfg, const OrderingOrdered& ordering)
{ {
GaussianFactorGraphUnordered gfgu; GaussianFactorGraph gfgu;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, gfg)
{ {
vector<std::pair<Key, Matrix> > terms; vector<std::pair<Key, Matrix> > terms;
const JacobianFactor& jacobian = dynamic_cast<const JacobianFactor&>(*factor); const JacobianFactorOrdered& jacobian = dynamic_cast<const JacobianFactorOrdered&>(*factor);
for(GaussianFactor::const_iterator term = jacobian.begin(); term != jacobian.end(); ++term) for(GaussianFactorOrdered::const_iterator term = jacobian.begin(); term != jacobian.end(); ++term)
{ {
terms.push_back(make_pair( terms.push_back(make_pair(
ordering.key(*term), ordering.key(*term),
jacobian.getA(term))); jacobian.getA(term)));
} }
gfgu.add(JacobianFactorUnordered(terms, jacobian.getb(), jacobian.get_model())); gfgu.add(JacobianFactor(terms, jacobian.getb(), jacobian.get_model()));
} }
return gfgu; return gfgu;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void compareSolutions(const VectorValues& orderedSoln, const Ordering& ordering, const VectorValuesUnordered& unorderedSoln) void compareSolutions(const VectorValuesOrdered& orderedSoln, const OrderingOrdered& ordering, const VectorValues& unorderedSoln)
{ {
if(orderedSoln.size() != unorderedSoln.size()) if(orderedSoln.size() != unorderedSoln.size())
{ {
@ -159,7 +159,7 @@ void compareSolutions(const VectorValues& orderedSoln, const Ordering& ordering,
else else
{ {
double maxErr = 0.0; double maxErr = 0.0;
BOOST_FOREACH(const VectorValuesUnordered::KeyValuePair& v, unorderedSoln) BOOST_FOREACH(const VectorValues::KeyValuePair& v, unorderedSoln)
{ {
Vector orderedV = orderedSoln[ordering[v.first]]; Vector orderedV = orderedSoln[ordering[v.first]];
maxErr = std::max(maxErr, (orderedV - v.second).cwiseAbs().maxCoeff()); maxErr = std::max(maxErr, (orderedV - v.second).cwiseAbs().maxCoeff());
@ -217,30 +217,30 @@ int main(int argc, char *argv[])
// Get linear graph // Get linear graph
cout << "Converting to unordered linear graph" << endl; cout << "Converting to unordered linear graph" << endl;
Ordering ordering = *isamsoln.orderingArbitrary(); OrderingOrdered ordering = *isamsoln.orderingArbitrary();
Ordering orderingCOLAMD = *nlfg.orderingCOLAMD(isamsoln); OrderingOrdered orderingCOLAMD = *nlfg.orderingCOLAMD(isamsoln);
GaussianFactorGraph gfg = *nlfg.linearize(isamsoln, ordering); GaussianFactorGraphOrdered gfg = *nlfg.linearize(isamsoln, ordering);
GaussianFactorGraphUnordered gfgu = convertToUnordered(gfg, ordering); GaussianFactorGraph gfgu = convertToUnordered(gfg, ordering);
//OrderingUnordered orderingUnordered; //Ordering orderingUnordered;
//for(Index j = 0; j < ordering.size(); ++j) //for(Index j = 0; j < ordering.size(); ++j)
// orderingUnordered.push_back(ordering.key(j)); // orderingUnordered.push_back(ordering.key(j));
// Solve linear graph // Solve linear graph
cout << "Optimizing unordered graph" << endl; cout << "Optimizing unordered graph" << endl;
VectorValuesUnordered unorderedSolnFinal; VectorValues unorderedSolnFinal;
{ {
gttic_(Solve_unordered); gttic_(Solve_unordered);
VectorValuesUnordered unorderedSoln; VectorValues unorderedSoln;
for(size_t i = 0; i < 1; ++i) { for(size_t i = 0; i < 1; ++i) {
gttic_(VariableIndex); gttic_(VariableIndexOrdered);
VariableIndexUnordered vi(gfgu); VariableIndex vi(gfgu);
gttoc_(VariableIndex); gttoc_(VariableIndexOrdered);
gttic_(COLAMD); gttic_(COLAMD);
OrderingUnordered orderingUnordered = OrderingUnordered::COLAMD(vi); Ordering orderingUnordered = Ordering::COLAMD(vi);
gttoc_(COLAMD); gttoc_(COLAMD);
gttic_(eliminate); gttic_(eliminate);
GaussianBayesTreeUnordered::shared_ptr bt = gfgu.eliminateMultifrontal(orderingUnordered); GaussianBayesTree::shared_ptr bt = gfgu.eliminateMultifrontal(orderingUnordered);
gttoc_(eliminate); gttoc_(eliminate);
gttic_(optimize); gttic_(optimize);
unorderedSoln = bt->optimize(); unorderedSoln = bt->optimize();
@ -252,22 +252,22 @@ int main(int argc, char *argv[])
// Solve linear graph with old code // Solve linear graph with old code
cout << "Optimizing using old ordered code" << endl; cout << "Optimizing using old ordered code" << endl;
VectorValues orderedSolnFinal; VectorValuesOrdered orderedSolnFinal;
{ {
Ordering orderingToUse = ordering; OrderingOrdered orderingToUse = ordering;
GaussianFactorGraph::shared_ptr orderedGraph = nlfg.linearize(isamsoln, *nlfg.orderingCOLAMD(isamsoln)); GaussianFactorGraphOrdered::shared_ptr orderedGraph = nlfg.linearize(isamsoln, *nlfg.orderingCOLAMD(isamsoln));
gttic_(Solve_ordered); gttic_(Solve_ordered);
VectorValues orderedSoln; VectorValuesOrdered orderedSoln;
for(size_t i = 0; i < 1; ++i) { for(size_t i = 0; i < 1; ++i) {
gttic_(VariableIndex); gttic_(VariableIndexOrdered);
boost::shared_ptr<VariableIndex> vi = boost::make_shared<VariableIndex>(gfg); boost::shared_ptr<VariableIndexOrdered> vi = boost::make_shared<VariableIndexOrdered>(gfg);
gttoc_(VariableIndex); gttoc_(VariableIndexOrdered);
gttic_(COLAMD); gttic_(COLAMD);
boost::shared_ptr<Permutation> permutation = inference::PermutationCOLAMD(*vi); boost::shared_ptr<Permutation> permutation = inference::PermutationCOLAMD(*vi);
orderingToUse.permuteInPlace(*permutation); orderingToUse.permuteInPlace(*permutation);
gttoc_(COLAMD); gttoc_(COLAMD);
gttic_(eliminate); gttic_(eliminate);
boost::shared_ptr<GaussianBayesTree> bt = GaussianMultifrontalSolver(*orderedGraph, true).eliminate(); boost::shared_ptr<GaussianBayesTreeOrdered> bt = GaussianMultifrontalSolver(*orderedGraph, true).eliminate();
gttoc_(eliminate); gttoc_(eliminate);
gttic_(optimize); gttic_(optimize);
orderedSoln = optimize(*bt); orderedSoln = optimize(*bt);
@ -280,8 +280,8 @@ int main(int argc, char *argv[])
// Compare results // Compare results
compareSolutions(orderedSolnFinal, orderingCOLAMD, unorderedSolnFinal); compareSolutions(orderedSolnFinal, orderingCOLAMD, unorderedSolnFinal);
//GaussianEliminationTreeUnordered(gfgu, orderingUnordered).print("ETree: "); //GaussianEliminationTree(gfgu, orderingUnordered).print("ETree: ");
//GaussianJunctionTreeUnordered(GaussianEliminationTreeUnordered(gfgu, OrderingUnordered::COLAMD(gfgu))).print("JTree: "); //GaussianJunctionTree(GaussianEliminationTree(gfgu, Ordering::COLAMD(gfgu))).print("JTree: ");
//gfgu.eliminateMultifrontal(orderingUnordered)->print("BayesTree: "); //gfgu.eliminateMultifrontal(orderingUnordered)->print("BayesTree: ");
tictoc_print_(); tictoc_print_();

52
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -22,11 +22,11 @@
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/OrderingOrdered.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNetOrdered.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraphOrdered.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
@ -38,10 +38,10 @@ int main() {
// [code below basically does SRIF with Cholesky] // [code below basically does SRIF with Cholesky]
// Create a factor graph to perform the inference // Create a factor graph to perform the inference
GaussianFactorGraph::shared_ptr linearFactorGraph(new GaussianFactorGraph); GaussianFactorGraphOrdered::shared_ptr linearFactorGraph(new GaussianFactorGraphOrdered);
// Create the desired ordering // Create the desired ordering
Ordering::shared_ptr ordering(new Ordering); OrderingOrdered::shared_ptr ordering(new OrderingOrdered);
// Create a structure to hold the linearization points // Create a structure to hold the linearization points
Values linearizationPoints; Values linearizationPoints;
@ -110,10 +110,10 @@ int main() {
// Solve the linear factor graph, converting it into a linear Bayes Network ( P(x0,x1) = P(x0|x1)*P(x1) ) // Solve the linear factor graph, converting it into a linear Bayes Network ( P(x0,x1) = P(x0|x1)*P(x1) )
GaussianSequentialSolver solver0(*linearFactorGraph); GaussianSequentialSolver solver0(*linearFactorGraph);
GaussianBayesNet::shared_ptr linearBayesNet = solver0.eliminate(); GaussianBayesNetOrdered::shared_ptr linearBayesNet = solver0.eliminate();
// Extract the current estimate of x1,P1 from the Bayes Network // Extract the current estimate of x1,P1 from the Bayes Network
VectorValues result = optimize(*linearBayesNet); VectorValuesOrdered result = optimize(*linearBayesNet);
Point2 x1_predict = linearizationPoints.at<Point2>(x1).retract(result[ordering->at(x1)]); Point2 x1_predict = linearizationPoints.at<Point2>(x1).retract(result[ordering->at(x1)]);
x1_predict.print("X1 Predict"); x1_predict.print("X1 Predict");
@ -123,7 +123,7 @@ int main() {
// Create a new, empty graph and add the prior from the previous step // Create a new, empty graph and add the prior from the previous step
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph); linearFactorGraph = GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step // Convert the root conditional, P(x1) in this case, into a Prior for the next step
// Some care must be done here, as the linearization point in future steps will be different // Some care must be done here, as the linearization point in future steps will be different
@ -138,13 +138,13 @@ int main() {
// -> b'' = b' - F(dx1' - dx1'') // -> b'' = b' - F(dx1' - dx1'')
// = || F*dx1'' - (b' - F(dx1' - dx1'')) ||^2 // = || F*dx1'' - (b' - F(dx1' - dx1'')) ||^2
// = || F*dx1'' - (b' - F(x_predict - x_inital)) ||^2 // = || F*dx1'' - (b' - F(x_predict - x_inital)) ||^2
const GaussianConditional::shared_ptr& cg0 = linearBayesNet->back(); const GaussianConditionalOrdered::shared_ptr& cg0 = linearBayesNet->back();
assert(cg0->nrFrontals() == 1); assert(cg0->nrFrontals() == 1);
assert(cg0->nrParents() == 0); assert(cg0->nrParents() == 0);
linearFactorGraph->add(0, cg0->get_R(), cg0->get_d() - cg0->get_R()*result[ordering->at(x1)], noiseModel::Diagonal::Sigmas(cg0->get_sigmas(), true)); linearFactorGraph->add(0, cg0->get_R(), cg0->get_d() - cg0->get_R()*result[ordering->at(x1)], noiseModel::Diagonal::Sigmas(cg0->get_sigmas(), true));
// Create the desired ordering // Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering); ordering = OrderingOrdered::shared_ptr(new OrderingOrdered);
ordering->insert(x1, 0); ordering->insert(x1, 0);
// Now, a measurement, z1, has been received, and the Kalman Filter should be "Updated"/"Corrected" // Now, a measurement, z1, has been received, and the Kalman Filter should be "Updated"/"Corrected"
@ -199,22 +199,22 @@ int main() {
// Wash, rinse, repeat for another time step // Wash, rinse, repeat for another time step
// Create a new, empty graph and add the prior from the previous step // Create a new, empty graph and add the prior from the previous step
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph); linearFactorGraph = GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step // Convert the root conditional, P(x1) in this case, into a Prior for the next step
// The linearization point of this prior must be moved to the new estimate of x, and the key/index needs to be reset to 0, // The linearization point of this prior must be moved to the new estimate of x, and the key/index needs to be reset to 0,
// the first key in the next iteration // the first key in the next iteration
const GaussianConditional::shared_ptr& cg1 = linearBayesNet->back(); const GaussianConditionalOrdered::shared_ptr& cg1 = linearBayesNet->back();
assert(cg1->nrFrontals() == 1); assert(cg1->nrFrontals() == 1);
assert(cg1->nrParents() == 0); assert(cg1->nrParents() == 0);
JacobianFactor tmpPrior1 = JacobianFactor(*cg1); JacobianFactorOrdered tmpPrior1 = JacobianFactorOrdered(*cg1);
linearFactorGraph->add(0, tmpPrior1.getA(tmpPrior1.begin()), tmpPrior1.getb() - tmpPrior1.getA(tmpPrior1.begin()) * result[ordering->at(x1)], tmpPrior1.get_model()); linearFactorGraph->add(0, tmpPrior1.getA(tmpPrior1.begin()), tmpPrior1.getb() - tmpPrior1.getA(tmpPrior1.begin()) * result[ordering->at(x1)], tmpPrior1.get_model());
// Create a key for the new state // Create a key for the new state
Symbol x2('x',2); Symbol x2('x',2);
// Create the desired ordering // Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering); ordering = OrderingOrdered::shared_ptr(new OrderingOrdered);
ordering->insert(x1, 0); ordering->insert(x1, 0);
ordering->insert(x2, 1); ordering->insert(x2, 1);
@ -243,17 +243,17 @@ int main() {
// Now add the next measurement // Now add the next measurement
// Create a new, empty graph and add the prior from the previous step // Create a new, empty graph and add the prior from the previous step
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph); linearFactorGraph = GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step // Convert the root conditional, P(x1) in this case, into a Prior for the next step
const GaussianConditional::shared_ptr& cg2 = linearBayesNet->back(); const GaussianConditionalOrdered::shared_ptr& cg2 = linearBayesNet->back();
assert(cg2->nrFrontals() == 1); assert(cg2->nrFrontals() == 1);
assert(cg2->nrParents() == 0); assert(cg2->nrParents() == 0);
JacobianFactor tmpPrior2 = JacobianFactor(*cg2); JacobianFactorOrdered tmpPrior2 = JacobianFactorOrdered(*cg2);
linearFactorGraph->add(0, tmpPrior2.getA(tmpPrior2.begin()), tmpPrior2.getb() - tmpPrior2.getA(tmpPrior2.begin()) * result[ordering->at(x2)], tmpPrior2.get_model()); linearFactorGraph->add(0, tmpPrior2.getA(tmpPrior2.begin()), tmpPrior2.getb() - tmpPrior2.getA(tmpPrior2.begin()) * result[ordering->at(x2)], tmpPrior2.get_model());
// Create the desired ordering // Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering); ordering = OrderingOrdered::shared_ptr(new OrderingOrdered);
ordering->insert(x2, 0); ordering->insert(x2, 0);
// And update using z2 ... // And update using z2 ...
@ -290,20 +290,20 @@ int main() {
// Wash, rinse, repeat for a third time step // Wash, rinse, repeat for a third time step
// Create a new, empty graph and add the prior from the previous step // Create a new, empty graph and add the prior from the previous step
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph); linearFactorGraph = GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step // Convert the root conditional, P(x1) in this case, into a Prior for the next step
const GaussianConditional::shared_ptr& cg3 = linearBayesNet->back(); const GaussianConditionalOrdered::shared_ptr& cg3 = linearBayesNet->back();
assert(cg3->nrFrontals() == 1); assert(cg3->nrFrontals() == 1);
assert(cg3->nrParents() == 0); assert(cg3->nrParents() == 0);
JacobianFactor tmpPrior3 = JacobianFactor(*cg3); JacobianFactorOrdered tmpPrior3 = JacobianFactorOrdered(*cg3);
linearFactorGraph->add(0, tmpPrior3.getA(tmpPrior3.begin()), tmpPrior3.getb() - tmpPrior3.getA(tmpPrior3.begin()) * result[ordering->at(x2)], tmpPrior3.get_model()); linearFactorGraph->add(0, tmpPrior3.getA(tmpPrior3.begin()), tmpPrior3.getb() - tmpPrior3.getA(tmpPrior3.begin()) * result[ordering->at(x2)], tmpPrior3.get_model());
// Create a key for the new state // Create a key for the new state
Symbol x3('x',3); Symbol x3('x',3);
// Create the desired ordering // Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering); ordering = OrderingOrdered::shared_ptr(new OrderingOrdered);
ordering->insert(x2, 0); ordering->insert(x2, 0);
ordering->insert(x3, 1); ordering->insert(x3, 1);
@ -332,17 +332,17 @@ int main() {
// Now add the next measurement // Now add the next measurement
// Create a new, empty graph and add the prior from the previous step // Create a new, empty graph and add the prior from the previous step
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph); linearFactorGraph = GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step // Convert the root conditional, P(x1) in this case, into a Prior for the next step
const GaussianConditional::shared_ptr& cg4 = linearBayesNet->back(); const GaussianConditionalOrdered::shared_ptr& cg4 = linearBayesNet->back();
assert(cg4->nrFrontals() == 1); assert(cg4->nrFrontals() == 1);
assert(cg4->nrParents() == 0); assert(cg4->nrParents() == 0);
JacobianFactor tmpPrior4 = JacobianFactor(*cg4); JacobianFactorOrdered tmpPrior4 = JacobianFactorOrdered(*cg4);
linearFactorGraph->add(0, tmpPrior4.getA(tmpPrior4.begin()), tmpPrior4.getb() - tmpPrior4.getA(tmpPrior4.begin()) * result[ordering->at(x3)], tmpPrior4.get_model()); linearFactorGraph->add(0, tmpPrior4.getA(tmpPrior4.begin()), tmpPrior4.getb() - tmpPrior4.getA(tmpPrior4.begin()) * result[ordering->at(x3)], tmpPrior4.get_model());
// Create the desired ordering // Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering); ordering = OrderingOrdered::shared_ptr(new OrderingOrdered);
ordering->insert(x3, 0); ordering->insert(x3, 0);
// And update using z3 ... // And update using z3 ...

4
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -45,14 +45,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
bool DecisionTreeFactor::equals(const This& other, double tol) const { bool DecisionTreeFactor::equals(const This& other, double tol) const {
return IndexFactor::equals(other, tol) && Potentials::equals(other, tol); return IndexFactorOrdered::equals(other, tol) && Potentials::equals(other, tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void DecisionTreeFactor::print(const string& s, void DecisionTreeFactor::print(const string& s,
const IndexFormatter& formatter) const { const IndexFormatter& formatter) const {
cout << s; cout << s;
IndexFactor::print("IndexFactor:",formatter); IndexFactorOrdered::print("IndexFactor:",formatter);
Potentials::print("Potentials:",formatter); Potentials::print("Potentials:",formatter);
} }

4
gtsam/discrete/DiscreteBayesNet.cpp
View File

@ -18,13 +18,13 @@
#include <gtsam/discrete/DiscreteBayesNet.h> #include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/discrete/DiscreteConditional.h> #include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/inference/BayesNet-inl.h> #include <gtsam/inference/BayesNetOrdered-inl.h>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
namespace gtsam { namespace gtsam {
// Explicitly instantiate so we don't have to include everywhere // Explicitly instantiate so we don't have to include everywhere
template class BayesNet<DiscreteConditional> ; template class BayesNetOrdered<DiscreteConditional> ;
/* ************************************************************************* */ /* ************************************************************************* */
void add_front(DiscreteBayesNet& bayesNet, const Signature& s) { void add_front(DiscreteBayesNet& bayesNet, const Signature& s) {

4
gtsam/discrete/DiscreteBayesNet.h
View File

@ -20,12 +20,12 @@
#include <vector> #include <vector>
#include <map> #include <map>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <gtsam/inference/BayesNet.h> #include <gtsam/inference/BayesNetOrdered.h>
#include <gtsam/discrete/DiscreteConditional.h> #include <gtsam/discrete/DiscreteConditional.h>
namespace gtsam { namespace gtsam {
typedef BayesNet<DiscreteConditional> DiscreteBayesNet; typedef BayesNetOrdered<DiscreteConditional> DiscreteBayesNet;
/** Add a DiscreteCondtional */ /** Add a DiscreteCondtional */
GTSAM_EXPORT void add(DiscreteBayesNet&, const Signature& s); GTSAM_EXPORT void add(DiscreteBayesNet&, const Signature& s);

12
gtsam/discrete/DiscreteConditional.cpp
View File

@ -37,14 +37,14 @@ namespace gtsam {
/* ******************************************************************************** */ /* ******************************************************************************** */
DiscreteConditional::DiscreteConditional(const size_t nrFrontals, DiscreteConditional::DiscreteConditional(const size_t nrFrontals,
const DecisionTreeFactor& f) : const DecisionTreeFactor& f) :
IndexConditional(f.keys(), nrFrontals), Potentials( IndexConditionalOrdered(f.keys(), nrFrontals), Potentials(
f / (*f.sum(nrFrontals))) { f / (*f.sum(nrFrontals))) {
} }
/* ******************************************************************************** */ /* ******************************************************************************** */
DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
const DecisionTreeFactor& marginal) : const DecisionTreeFactor& marginal) :
IndexConditional(joint.keys(), joint.size() - marginal.size()), Potentials( IndexConditionalOrdered(joint.keys(), joint.size() - marginal.size()), Potentials(
ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal) { ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal) {
if (ISDEBUG("DiscreteConditional::DiscreteConditional")) if (ISDEBUG("DiscreteConditional::DiscreteConditional"))
cout << (firstFrontalKey()) << endl; //TODO Print all keys cout << (firstFrontalKey()) << endl; //TODO Print all keys
@ -52,20 +52,20 @@ namespace gtsam {
/* ******************************************************************************** */ /* ******************************************************************************** */
DiscreteConditional::DiscreteConditional(const Signature& signature) : DiscreteConditional::DiscreteConditional(const Signature& signature) :
IndexConditional(signature.indices(), 1), Potentials( IndexConditionalOrdered(signature.indices(), 1), Potentials(
signature.discreteKeysParentsFirst(), signature.cpt()) { signature.discreteKeysParentsFirst(), signature.cpt()) {
} }
/* ******************************************************************************** */ /* ******************************************************************************** */
void DiscreteConditional::print(const std::string& s, const IndexFormatter& formatter) const { void DiscreteConditional::print(const std::string& s, const IndexFormatter& formatter) const {
std::cout << s << std::endl; std::cout << s << std::endl;
IndexConditional::print(s, formatter); IndexConditionalOrdered::print(s, formatter);
Potentials::print(s); Potentials::print(s);
} }
/* ******************************************************************************** */ /* ******************************************************************************** */
bool DiscreteConditional::equals(const DiscreteConditional& other, double tol) const { bool DiscreteConditional::equals(const DiscreteConditional& other, double tol) const {
return IndexConditional::equals(other, tol) return IndexConditionalOrdered::equals(other, tol)
&& Potentials::equals(other, tol); && Potentials::equals(other, tol);
} }
@ -197,7 +197,7 @@ namespace gtsam {
/* ******************************************************************************** */ /* ******************************************************************************** */
void DiscreteConditional::permuteWithInverse(const Permutation& inversePermutation){ void DiscreteConditional::permuteWithInverse(const Permutation& inversePermutation){
IndexConditional::permuteWithInverse(inversePermutation); IndexConditionalOrdered::permuteWithInverse(inversePermutation);
Potentials::permuteWithInverse(inversePermutation); Potentials::permuteWithInverse(inversePermutation);
} }

6
gtsam/discrete/DiscreteConditional.h
View File

@ -20,7 +20,7 @@
#include <gtsam/discrete/DecisionTreeFactor.h> #include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/Signature.h> #include <gtsam/discrete/Signature.h>
#include <gtsam/inference/IndexConditional.h> #include <gtsam/inference/IndexConditionalOrdered.h>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
@ -30,13 +30,13 @@ namespace gtsam {
* Discrete Conditional Density * Discrete Conditional Density
* Derives from DecisionTreeFactor * Derives from DecisionTreeFactor
*/ */
class GTSAM_EXPORT DiscreteConditional: public IndexConditional, public Potentials { class GTSAM_EXPORT DiscreteConditional: public IndexConditionalOrdered, public Potentials {
public: public:
// typedefs needed to play nice with gtsam // typedefs needed to play nice with gtsam
typedef DiscreteFactor FactorType; typedef DiscreteFactor FactorType;
typedef boost::shared_ptr<DiscreteConditional> shared_ptr; typedef boost::shared_ptr<DiscreteConditional> shared_ptr;
typedef IndexConditional Base; typedef IndexConditionalOrdered Base;
/** A map from keys to values */ /** A map from keys to values */
typedef Assignment<Index> Values; typedef Assignment<Index> Values;

18
gtsam/discrete/DiscreteFactor.h
View File

@ -19,7 +19,7 @@
#pragma once #pragma once
#include <gtsam/discrete/Assignment.h> #include <gtsam/discrete/Assignment.h>
#include <gtsam/inference/IndexFactor.h> #include <gtsam/inference/IndexFactorOrdered.h>
namespace gtsam { namespace gtsam {
@ -30,7 +30,7 @@ namespace gtsam {
* Base class for discrete probabilistic factors * Base class for discrete probabilistic factors
* The most general one is the derived DecisionTreeFactor * The most general one is the derived DecisionTreeFactor
*/ */
class GTSAM_EXPORT DiscreteFactor: public IndexFactor { class GTSAM_EXPORT DiscreteFactor: public IndexFactorOrdered {
public: public:
@ -47,23 +47,23 @@ namespace gtsam {
/// Construct n-way factor /// Construct n-way factor
DiscreteFactor(const std::vector<Index>& js) : DiscreteFactor(const std::vector<Index>& js) :
IndexFactor(js) { IndexFactorOrdered(js) {
} }
/// Construct unary factor /// Construct unary factor
DiscreteFactor(Index j) : DiscreteFactor(Index j) :
IndexFactor(j) { IndexFactorOrdered(j) {
} }
/// Construct binary factor /// Construct binary factor
DiscreteFactor(Index j1, Index j2) : DiscreteFactor(Index j1, Index j2) :
IndexFactor(j1, j2) { IndexFactorOrdered(j1, j2) {
} }
/// construct from container /// construct from container
template<class KeyIterator> template<class KeyIterator>
DiscreteFactor(KeyIterator beginKey, KeyIterator endKey) : DiscreteFactor(KeyIterator beginKey, KeyIterator endKey) :
IndexFactor(beginKey, endKey) { IndexFactorOrdered(beginKey, endKey) {
} }
public: public:
@ -85,7 +85,7 @@ namespace gtsam {
virtual void print(const std::string& s = "DiscreteFactor\n", virtual void print(const std::string& s = "DiscreteFactor\n",
const IndexFormatter& formatter const IndexFormatter& formatter
=DefaultIndexFormatter) const { =DefaultIndexFormatter) const {
IndexFactor::print(s,formatter); IndexFactorOrdered::print(s,formatter);
} }
/// @} /// @}
@ -105,14 +105,14 @@ namespace gtsam {
* to already be inverted. * to already be inverted.
*/ */
virtual void permuteWithInverse(const Permutation& inversePermutation){ virtual void permuteWithInverse(const Permutation& inversePermutation){
IndexFactor::permuteWithInverse(inversePermutation); IndexFactorOrdered::permuteWithInverse(inversePermutation);
} }
/** /**
* Apply a reduction, which is a remapping of variable indices. * Apply a reduction, which is a remapping of variable indices.
*/ */
virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { virtual void reduceWithInverse(const internal::Reduction& inverseReduction) {
IndexFactor::reduceWithInverse(inverseReduction); IndexFactorOrdered::reduceWithInverse(inverseReduction);
} }
/// @} /// @}

12
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -19,14 +19,14 @@
//#define ENABLE_TIMING //#define ENABLE_TIMING
#include <gtsam/discrete/DiscreteFactorGraph.h> #include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteConditional.h> #include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/inference/EliminationTree-inl.h> #include <gtsam/inference/EliminationTreeOrdered-inl.h>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
namespace gtsam { namespace gtsam {
// Explicitly instantiate so we don't have to include everywhere // Explicitly instantiate so we don't have to include everywhere
template class FactorGraph<DiscreteFactor> ; template class FactorGraphOrdered<DiscreteFactor> ;
template class EliminationTree<DiscreteFactor> ; template class EliminationTreeOrdered<DiscreteFactor> ;
/* ************************************************************************* */ /* ************************************************************************* */
DiscreteFactorGraph::DiscreteFactorGraph() { DiscreteFactorGraph::DiscreteFactorGraph() {
@ -34,8 +34,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
DiscreteFactorGraph::DiscreteFactorGraph( DiscreteFactorGraph::DiscreteFactorGraph(
const BayesNet<DiscreteConditional>& bayesNet) : const BayesNetOrdered<DiscreteConditional>& bayesNet) :
FactorGraph<DiscreteFactor>(bayesNet) { FactorGraphOrdered<DiscreteFactor>(bayesNet) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -95,7 +95,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> // std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
EliminateDiscrete(const FactorGraph<DiscreteFactor>& factors, size_t num) { EliminateDiscrete(const FactorGraphOrdered<DiscreteFactor>& factors, size_t num) {
// PRODUCT: multiply all factors // PRODUCT: multiply all factors
gttic(product); gttic(product);

10
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -20,13 +20,13 @@
#include <gtsam/discrete/DecisionTreeFactor.h> #include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteBayesNet.h> #include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraphOrdered.h>
#include <gtsam/base/FastSet.h> #include <gtsam/base/FastSet.h>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
namespace gtsam { namespace gtsam {
class DiscreteFactorGraph: public FactorGraph<DiscreteFactor> { class DiscreteFactorGraph: public FactorGraphOrdered<DiscreteFactor> {
public: public:
/** A map from keys to values */ /** A map from keys to values */
@ -39,12 +39,12 @@ public:
/** Constructor from a factor graph of GaussianFactor or a derived type */ /** Constructor from a factor graph of GaussianFactor or a derived type */
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
DiscreteFactorGraph(const FactorGraph<DERIVEDFACTOR>& fg) { DiscreteFactorGraph(const FactorGraphOrdered<DERIVEDFACTOR>& fg) {
push_back(fg); push_back(fg);
} }
/** construct from a BayesNet */ /** construct from a BayesNet */
GTSAM_EXPORT DiscreteFactorGraph(const BayesNet<DiscreteConditional>& bayesNet); GTSAM_EXPORT DiscreteFactorGraph(const BayesNetOrdered<DiscreteConditional>& bayesNet);
template<class SOURCE> template<class SOURCE>
void add(const DiscreteKey& j, SOURCE table) { void add(const DiscreteKey& j, SOURCE table) {
@ -90,7 +90,7 @@ public:
/** Main elimination function for DiscreteFactorGraph */ /** Main elimination function for DiscreteFactorGraph */
GTSAM_EXPORT std::pair<boost::shared_ptr<DiscreteConditional>, DecisionTreeFactor::shared_ptr> GTSAM_EXPORT std::pair<boost::shared_ptr<DiscreteConditional>, DecisionTreeFactor::shared_ptr>
EliminateDiscrete(const FactorGraph<DiscreteFactor>& factors, EliminateDiscrete(const FactorGraphOrdered<DiscreteFactor>& factors,
size_t nrFrontals = 1); size_t nrFrontals = 1);
} // namespace gtsam } // namespace gtsam

4
gtsam/discrete/DiscreteMarginals.h
View File

@ -32,7 +32,7 @@ namespace gtsam {
protected: protected:
BayesTree<DiscreteConditional> bayesTree_; BayesTreeOrdered<DiscreteConditional> bayesTree_;
public: public:
@ -40,7 +40,7 @@ namespace gtsam {
* @param graph The factor graph defining the full joint density on all variables. * @param graph The factor graph defining the full joint density on all variables.
*/ */
DiscreteMarginals(const DiscreteFactorGraph& graph) { DiscreteMarginals(const DiscreteFactorGraph& graph) {
typedef JunctionTree<DiscreteFactorGraph> DiscreteJT; typedef JunctionTreeOrdered<DiscreteFactorGraph> DiscreteJT;
GenericMultifrontalSolver<DiscreteFactor, DiscreteJT> solver(graph); GenericMultifrontalSolver<DiscreteFactor, DiscreteJT> solver(graph);
bayesTree_ = *solver.eliminate(&EliminateDiscrete); bayesTree_ = *solver.eliminate(&EliminateDiscrete);
} }

10
gtsam/discrete/DiscreteSequentialSolver.h
View File

@ -47,7 +47,7 @@ namespace gtsam {
* Construct the solver for a factor graph. This builds the elimination * Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the work of elimination. * tree, which already does some of the work of elimination.
*/ */
DiscreteSequentialSolver(const FactorGraph<DiscreteFactor>& factorGraph) : DiscreteSequentialSolver(const FactorGraphOrdered<DiscreteFactor>& factorGraph) :
Base(factorGraph) { Base(factorGraph) {
} }
@ -57,12 +57,12 @@ namespace gtsam {
* is the fastest. * is the fastest.
*/ */
DiscreteSequentialSolver( DiscreteSequentialSolver(
const FactorGraph<DiscreteFactor>::shared_ptr& factorGraph, const FactorGraphOrdered<DiscreteFactor>::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex) : const VariableIndexOrdered::shared_ptr& variableIndex) :
Base(factorGraph, variableIndex) { Base(factorGraph, variableIndex) {
} }
const EliminationTree<DiscreteFactor>& eliminationTree() const { const EliminationTreeOrdered<DiscreteFactor>& eliminationTree() const {
return *eliminationTree_; return *eliminationTree_;
} }
@ -70,7 +70,7 @@ namespace gtsam {
* Eliminate the factor graph sequentially. Uses a column elimination tree * Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate. * to recursively eliminate.
*/ */
BayesNet<DiscreteConditional>::shared_ptr eliminate() const { BayesNetOrdered<DiscreteConditional>::shared_ptr eliminate() const {
return Base::eliminate(&EliminateDiscrete); return Base::eliminate(&EliminateDiscrete);
} }

2
gtsam/discrete/Potentials.h
View File

@ -19,7 +19,7 @@
#include <gtsam/discrete/AlgebraicDecisionTree.h> #include <gtsam/discrete/AlgebraicDecisionTree.h>
#include <gtsam/discrete/DiscreteKey.h> #include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/inference/Permutation.h> #include <gtsam/inference/PermutationOrdered.h>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <set> #include <set>

10
gtsam/discrete/tests/testDiscreteBayesTree.cpp
View File

@ -17,7 +17,7 @@
#include <gtsam/discrete/DiscreteBayesNet.h> #include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/discrete/DiscreteFactorGraph.h> #include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/inference/BayesTree.h> #include <gtsam/inference/BayesTreeOrdered.h>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
using namespace boost::assign; using namespace boost::assign;
@ -32,13 +32,13 @@ static bool debug = false;
/** /**
* Custom clique class to debug shortcuts * Custom clique class to debug shortcuts
*/ */
class Clique: public BayesTreeCliqueBase<Clique, DiscreteConditional> { class Clique: public BayesTreeCliqueBaseOrdered<Clique, DiscreteConditional> {
protected: protected:
public: public:
typedef BayesTreeCliqueBase<Clique, DiscreteConditional> Base; typedef BayesTreeCliqueBaseOrdered<Clique, DiscreteConditional> Base;
typedef boost::shared_ptr<Clique> shared_ptr; typedef boost::shared_ptr<Clique> shared_ptr;
// Constructors // Constructors
@ -56,7 +56,7 @@ public:
/// print index signature only /// print index signature only
void printSignature(const std::string& s = "Clique: ", void printSignature(const std::string& s = "Clique: ",
const IndexFormatter& indexFormatter = DefaultIndexFormatter) const { const IndexFormatter& indexFormatter = DefaultIndexFormatter) const {
((IndexConditional::shared_ptr) conditional_)->print(s, indexFormatter); ((IndexConditionalOrdered::shared_ptr) conditional_)->print(s, indexFormatter);
} }
/// evaluate value of sub-tree /// evaluate value of sub-tree
@ -70,7 +70,7 @@ public:
}; };
typedef BayesTree<DiscreteConditional, Clique> DiscreteBayesTree; typedef BayesTreeOrdered<DiscreteConditional, Clique> DiscreteBayesTree;
/* ************************************************************************* */ /* ************************************************************************* */
double evaluate(const DiscreteBayesTree& tree, double evaluate(const DiscreteBayesTree& tree,

12
gtsam/discrete/tests/testDiscreteFactorGraph.cpp
View File

@ -60,7 +60,7 @@ TEST_UNSAFE( DiscreteFactorGraph, debugScheduler) {
// //
DiscreteSequentialSolver solver(graph); DiscreteSequentialSolver solver(graph);
// solver.print("solver:"); // solver.print("solver:");
EliminationTree<DiscreteFactor> eliminationTree = solver.eliminationTree(); EliminationTreeOrdered<DiscreteFactor> eliminationTree = solver.eliminationTree();
// eliminationTree.print("Elimination tree: "); // eliminationTree.print("Elimination tree: ");
eliminationTree.eliminate(EliminateDiscrete); eliminationTree.eliminate(EliminateDiscrete);
// solver.optimize(); // solver.optimize();
@ -155,7 +155,7 @@ TEST( DiscreteFactorGraph, test)
// GTSAM_PRINT(expected); // GTSAM_PRINT(expected);
// Test elimination tree // Test elimination tree
const EliminationTree<DiscreteFactor>& etree = solver.eliminationTree(); const EliminationTreeOrdered<DiscreteFactor>& etree = solver.eliminationTree();
DiscreteBayesNet::shared_ptr actual = etree.eliminate(&EliminateDiscrete); DiscreteBayesNet::shared_ptr actual = etree.eliminate(&EliminateDiscrete);
EXPECT(assert_equal(expected, *actual)); EXPECT(assert_equal(expected, *actual));
@ -221,16 +221,16 @@ TEST( DiscreteFactorGraph, testMPE_Darwiche09book_p244)
// EXPECT_DOUBLES_EQUAL(0.4, (*root)(*actualMPE), 1e-9); // EXPECT_DOUBLES_EQUAL(0.4, (*root)(*actualMPE), 1e-9);
// Let us create the Bayes tree here, just for fun, because we don't use it now // Let us create the Bayes tree here, just for fun, because we don't use it now
typedef JunctionTree<DiscreteFactorGraph> JT; typedef JunctionTreeOrdered<DiscreteFactorGraph> JT;
GenericMultifrontalSolver<DiscreteFactor, JT> solver(graph); GenericMultifrontalSolver<DiscreteFactor, JT> solver(graph);
BayesTree<DiscreteConditional>::shared_ptr bayesTree = solver.eliminate(&EliminateDiscrete); BayesTreeOrdered<DiscreteConditional>::shared_ptr bayesTree = solver.eliminate(&EliminateDiscrete);
// bayesTree->print("Bayes Tree"); // bayesTree->print("Bayes Tree");
EXPECT_LONGS_EQUAL(2,bayesTree->size()); EXPECT_LONGS_EQUAL(2,bayesTree->size());
#ifdef OLD #ifdef OLD
// Create the elimination tree manually // Create the elimination tree manually
VariableIndex structure(graph); VariableIndexOrdered structure(graph);
typedef EliminationTree<DiscreteFactor> ETree; typedef EliminationTreeOrdered<DiscreteFactor> ETree;
ETree::shared_ptr eTree = ETree::Create(graph, structure); ETree::shared_ptr eTree = ETree::Create(graph, structure);
//eTree->print(">>>>>>>>>>> Elimination Tree <<<<<<<<<<<<<<<<<"); //eTree->print(">>>>>>>>>>> Elimination Tree <<<<<<<<<<<<<<<<<");

4
gtsam/discrete/tests/testDiscreteMarginals.cpp
View File

@ -119,9 +119,9 @@ TEST_UNSAFE( DiscreteMarginals, truss ) {
graph.add(key[0] & key[2] & key[4],"2 2 2 2 1 1 1 1"); graph.add(key[0] & key[2] & key[4],"2 2 2 2 1 1 1 1");
graph.add(key[1] & key[3] & key[4],"1 1 1 1 2 2 2 2"); graph.add(key[1] & key[3] & key[4],"1 1 1 1 2 2 2 2");
graph.add(key[2] & key[3] & key[4],"1 1 1 1 1 1 1 1"); graph.add(key[2] & key[3] & key[4],"1 1 1 1 1 1 1 1");
typedef JunctionTree<DiscreteFactorGraph> JT; typedef JunctionTreeOrdered<DiscreteFactorGraph> JT;
GenericMultifrontalSolver<DiscreteFactor, JT> solver(graph); GenericMultifrontalSolver<DiscreteFactor, JT> solver(graph);
BayesTree<DiscreteConditional>::shared_ptr bayesTree = solver.eliminate(&EliminateDiscrete); BayesTreeOrdered<DiscreteConditional>::shared_ptr bayesTree = solver.eliminate(&EliminateDiscrete);
// bayesTree->print("Bayes Tree"); // bayesTree->print("Bayes Tree");
typedef BayesTreeClique<DiscreteConditional> Clique; typedef BayesTreeClique<DiscreteConditional> Clique;

8
gtsam/inference/BayesNetUnordered-inst.h → gtsam/inference/BayesNet-inst.h
View File

@ -18,8 +18,8 @@
#pragma once #pragma once
#include <gtsam/inference/FactorGraphUnordered-inst.h> #include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/inference/BayesNetUnordered.h> #include <gtsam/inference/BayesNet.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <fstream> #include <fstream>
@ -28,14 +28,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
void BayesNetUnordered<CONDITIONAL>::print(const std::string& s, const KeyFormatter& formatter) const void BayesNet<CONDITIONAL>::print(const std::string& s, const KeyFormatter& formatter) const
{ {
Base::print(s, formatter); Base::print(s, formatter);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
void BayesNetUnordered<CONDITIONAL>::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const void BayesNet<CONDITIONAL>::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const
{ {
std::ofstream of(s.c_str()); std::ofstream of(s.c_str());
of << "digraph G{\n"; of << "digraph G{\n";

316
gtsam/inference/BayesNet.h
View File

@ -1,244 +1,72 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file BayesNet.h * @file BayesNet.h
* @brief Bayes network * @brief Bayes network
* @author Frank Dellaert * @author Frank Dellaert
*/ * @author Richard Roberts
*/
// \callgraph
#pragma once
#pragma once
#include <boost/shared_ptr.hpp>
#include <list>
#include <boost/shared_ptr.hpp> #include <gtsam/inference/FactorGraph.h>
#include <boost/serialization/nvp.hpp>
#include <boost/assign/list_inserter.hpp> namespace gtsam {
#include <boost/lexical_cast.hpp>
/**
#include <gtsam/global_includes.h> * A BayesNet is a tree of conditionals, stored in elimination order.
#include <gtsam/base/FastList.h> *
#include <gtsam/inference/Permutation.h> * todo: how to handle Bayes nets with an optimize function? Currently using global functions.
* \nosubgrouping
namespace gtsam { */
template<class CONDITIONAL>
/** class BayesNet : public FactorGraph<CONDITIONAL> {
* A BayesNet is a list of conditionals, stored in elimination order, i.e.
* leaves first, parents last. GaussianBayesNet and SymbolicBayesNet are private:
* defined as typedefs of this class, using GaussianConditional and
* IndexConditional as the CONDITIONAL template argument. typedef FactorGraph<CONDITIONAL> Base;
*
* todo: Symbolic using Index is a misnomer. public:
* todo: how to handle Bayes nets with an optimize function? Currently using global functions. typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional; ///< A shared pointer to a conditional
* \nosubgrouping
*/ protected:
template<class CONDITIONAL> /// @name Standard Constructors
class BayesNet { /// @{
public: /** Default constructor as an empty BayesNet */
BayesNet() {};
typedef typename boost::shared_ptr<BayesNet<CONDITIONAL> > shared_ptr;
/** Construct from iterator over conditionals */
/** We store shared pointers to Conditional densities */ template<typename ITERATOR>
typedef typename CONDITIONAL::KeyType KeyType; BayesNet(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {}
typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional;
typedef typename boost::shared_ptr<const CONDITIONAL> const_sharedConditional; /// @}
typedef typename std::list<sharedConditional> Conditionals;
public:
typedef typename Conditionals::iterator iterator; /// @name Testable
typedef typename Conditionals::reverse_iterator reverse_iterator; /// @{
typedef typename Conditionals::const_iterator const_iterator;
typedef typename Conditionals::const_reverse_iterator const_reverse_iterator; /** print out graph */
void print(const std::string& s = "BayesNet",
protected: const KeyFormatter& formatter = DefaultKeyFormatter) const;
/** /// @}
* Conditional densities are stored in reverse topological sort order (i.e., leaves first,
* parents last), which corresponds to the elimination ordering if so obtained, /// @name Standard Interface
* and is consistent with the column (block) ordering of an upper triangular matrix. /// @{
*/
Conditionals conditionals_; void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
};
public:
}
/// @name Standard Constructors
/// @{
/** Default constructor as an empty BayesNet */
BayesNet() {};
/** convert from a derived type */
template<class DERIVEDCONDITIONAL>
BayesNet(const BayesNet<DERIVEDCONDITIONAL>& bn) {
conditionals_.assign(bn.begin(), bn.end());
}
/** BayesNet with 1 conditional */
explicit BayesNet(const sharedConditional& conditional) { push_back(conditional); }
/// @}
/// @name Testable
/// @{
/** print */
void print(const std::string& s = "",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** check equality */
bool equals(const BayesNet& other, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
/** size is the number of nodes */
size_t size() const {
return conditionals_.size();
}
/** @return true if there are no conditionals/nodes */
bool empty() const {
return conditionals_.empty();
}
/** print statistics */
void printStats(const std::string& s = "") const;
/** save dot graph */
void saveGraph(const std::string &s, const IndexFormatter& indexFormatter =
DefaultIndexFormatter) const;
/** return keys in reverse topological sort order, i.e., elimination order */
FastList<Index> ordering() const;
/** SLOW O(n) random access to Conditional by key */
sharedConditional operator[](Index key) const;
/** return last node in ordering */
boost::shared_ptr<const CONDITIONAL> front() const { return conditionals_.front(); }
/** return last node in ordering */
boost::shared_ptr<const CONDITIONAL> back() const { return conditionals_.back(); }
/** return iterators. FD: breaks encapsulation? */
const_iterator begin() const {return conditionals_.begin();} ///<TODO: comment
const_iterator end() const {return conditionals_.end();} ///<TODO: comment
const_reverse_iterator rbegin() const {return conditionals_.rbegin();} ///<TODO: comment
const_reverse_iterator rend() const {return conditionals_.rend();} ///<TODO: comment
/** Find an iterator pointing to the conditional where the specified key
* appears as a frontal variable, or end() if no conditional contains this
* key. Running time is approximately \f$ O(n) \f$ in the number of
* conditionals in the BayesNet.
* @param key The index to find in the frontal variables of a conditional.
*/
const_iterator find(Index key) const;
/// @}
/// @name Advanced Interface
/// @{
/**
* Remove any leaf conditional. The conditional to remove is specified by
* iterator. To find the iterator pointing to the conditional containing a
* particular key, use find(), which has \f$ O(n) \f$ complexity. The
* popLeaf function by itself has \f$ O(1) \f$ complexity.
*
* If gtsam is compiled with GTSAM_EXTRA_CONSISTENCY_CHECKS defined, this
* function will check that the node is indeed a leaf, but otherwise will
* not check, because the check has \f$ O(n^2) \f$ complexity.
*
* Example 1:
\code
// Remove a leaf node with a known conditional
GaussianBayesNet gbn = ...
GaussianBayesNet::iterator leafConditional = ...
gbn.popLeaf(leafConditional);
\endcode
* Example 2:
\code
// Remove the leaf node containing variable index 14
GaussianBayesNet gbn = ...
gbn.popLeaf(gbn.find(14));
\endcode
* @param conditional The iterator pointing to the leaf conditional to remove
*/
void popLeaf(iterator conditional);
/** return last node in ordering */
sharedConditional& front() { return conditionals_.front(); }
/** return last node in ordering */
sharedConditional& back() { return conditionals_.back(); }
/** Find an iterator pointing to the conditional where the specified key
* appears as a frontal variable, or end() if no conditional contains this
* key. Running time is approximately \f$ O(n) \f$ in the number of
* conditionals in the BayesNet.
* @param key The index to find in the frontal variables of a conditional.
*/
iterator find(Index key);
/** push_back: use reverse topological sort (i.e. parents last / elimination order) */
inline void push_back(const sharedConditional& conditional) {
conditionals_.push_back(conditional);
}
/** push_front: use topological sort (i.e. parents first / reverse elimination order) */
inline void push_front(const sharedConditional& conditional) {
conditionals_.push_front(conditional);
}
/// push_back an entire Bayes net
void push_back(const BayesNet<CONDITIONAL>& bn);
/// push_front an entire Bayes net
void push_front(const BayesNet<CONDITIONAL>& bn);
/** += syntax for push_back, e.g. bayesNet += c1, c2, c3
* @param conditional The conditional to add to the back of the BayesNet
*/
boost::assign::list_inserter<boost::assign_detail::call_push_back<BayesNet<CONDITIONAL> >, sharedConditional>
operator+=(const sharedConditional& conditional) {
return boost::assign::make_list_inserter(boost::assign_detail::call_push_back<BayesNet<CONDITIONAL> >(*this))(conditional); }
/**
* pop_front: remove node at the bottom, used in marginalization
* For example P(ABC)=P(A|BC)P(B|C)P(C) becomes P(BC)=P(B|C)P(C)
*/
void pop_front() {conditionals_.pop_front();}
/** Permute the variables in the BayesNet */
void permuteWithInverse(const Permutation& inversePermutation);
iterator begin() {return conditionals_.begin();} ///<TODO: comment
iterator end() {return conditionals_.end();} ///<TODO: comment
reverse_iterator rbegin() {return conditionals_.rbegin();} ///<TODO: comment
reverse_iterator rend() {return conditionals_.rend();} ///<TODO: comment
/** saves the bayes to a text file in GraphViz format */
// void saveGraph(const std::string& s) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(conditionals_);
}
/// @}
}; // BayesNet
} // namespace gtsam
#include <gtsam/inference/BayesNet-inl.h>

24
gtsam/inference/BayesNet-inl.h → gtsam/inference/BayesNetOrdered-inl.h
View File

@ -34,7 +34,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::print(const std::string& s, void BayesNetOrdered<CONDITIONAL>::print(const std::string& s,
const IndexFormatter& formatter) const { const IndexFormatter& formatter) const {
std::cout << s; std::cout << s;
BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_) BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_)
@ -43,7 +43,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
bool BayesNet<CONDITIONAL>::equals(const BayesNet& cbn, double tol) const { bool BayesNetOrdered<CONDITIONAL>::equals(const BayesNetOrdered& cbn, double tol) const {
if (size() != cbn.size()) if (size() != cbn.size())
return false; return false;
return std::equal(conditionals_.begin(), conditionals_.end(), return std::equal(conditionals_.begin(), conditionals_.end(),
@ -52,7 +52,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::printStats(const std::string& s) const { void BayesNetOrdered<CONDITIONAL>::printStats(const std::string& s) const {
const size_t n = conditionals_.size(); const size_t n = conditionals_.size();
size_t max_size = 0; size_t max_size = 0;
@ -69,7 +69,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::saveGraph(const std::string &s, void BayesNetOrdered<CONDITIONAL>::saveGraph(const std::string &s,
const IndexFormatter& indexFormatter) const { const IndexFormatter& indexFormatter) const {
std::ofstream of(s.c_str()); std::ofstream of(s.c_str());
of << "digraph G{\n"; of << "digraph G{\n";
@ -88,7 +88,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::const_iterator BayesNet<CONDITIONAL>::find( typename BayesNetOrdered<CONDITIONAL>::const_iterator BayesNetOrdered<CONDITIONAL>::find(
Index key) const { Index key) const {
for (const_iterator it = begin(); it != end(); ++it) for (const_iterator it = begin(); it != end(); ++it)
if (std::find((*it)->beginFrontals(), (*it)->endFrontals(), key) if (std::find((*it)->beginFrontals(), (*it)->endFrontals(), key)
@ -99,7 +99,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::iterator BayesNet<CONDITIONAL>::find( typename BayesNetOrdered<CONDITIONAL>::iterator BayesNetOrdered<CONDITIONAL>::find(
Index key) { Index key) {
for (iterator it = begin(); it != end(); ++it) for (iterator it = begin(); it != end(); ++it)
if (std::find((*it)->beginFrontals(), (*it)->endFrontals(), key) if (std::find((*it)->beginFrontals(), (*it)->endFrontals(), key)
@ -110,7 +110,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::permuteWithInverse( void BayesNetOrdered<CONDITIONAL>::permuteWithInverse(
const Permutation& inversePermutation) { const Permutation& inversePermutation) {
BOOST_FOREACH(sharedConditional conditional, conditionals_) { BOOST_FOREACH(sharedConditional conditional, conditionals_) {
conditional->permuteWithInverse(inversePermutation); conditional->permuteWithInverse(inversePermutation);
@ -119,21 +119,21 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::push_back(const BayesNet<CONDITIONAL>& bn) { void BayesNetOrdered<CONDITIONAL>::push_back(const BayesNetOrdered<CONDITIONAL>& bn) {
BOOST_FOREACH(sharedConditional conditional,bn.conditionals_) BOOST_FOREACH(sharedConditional conditional,bn.conditionals_)
push_back(conditional); push_back(conditional);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::push_front(const BayesNet<CONDITIONAL>& bn) { void BayesNetOrdered<CONDITIONAL>::push_front(const BayesNetOrdered<CONDITIONAL>& bn) {
BOOST_FOREACH(sharedConditional conditional,bn.conditionals_) BOOST_FOREACH(sharedConditional conditional,bn.conditionals_)
push_front(conditional); push_front(conditional);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::popLeaf(iterator conditional) { void BayesNetOrdered<CONDITIONAL>::popLeaf(iterator conditional) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
BOOST_FOREACH(typename CONDITIONAL::shared_ptr checkConditional, conditionals_) { BOOST_FOREACH(typename CONDITIONAL::shared_ptr checkConditional, conditionals_) {
BOOST_FOREACH(Index key, (*conditional)->frontals()) { BOOST_FOREACH(Index key, (*conditional)->frontals()) {
@ -148,7 +148,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
FastList<Index> BayesNet<CONDITIONAL>::ordering() const { FastList<Index> BayesNetOrdered<CONDITIONAL>::ordering() const {
FastList<Index> ord; FastList<Index> ord;
BOOST_FOREACH(sharedConditional conditional,conditionals_) BOOST_FOREACH(sharedConditional conditional,conditionals_)
ord.insert(ord.begin(), conditional->beginFrontals(), ord.insert(ord.begin(), conditional->beginFrontals(),
@ -174,7 +174,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::sharedConditional BayesNet<CONDITIONAL>::operator[]( typename BayesNetOrdered<CONDITIONAL>::sharedConditional BayesNetOrdered<CONDITIONAL>::operator[](
Index key) const { Index key) const {
BOOST_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) { BOOST_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) {
typename CONDITIONAL::const_iterator it = std::find( typename CONDITIONAL::const_iterator it = std::find(

244
gtsam/inference/BayesNetOrdered.h Normal file
View File

@ -0,0 +1,244 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file BayesNet.h
* @brief Bayes network
* @author Frank Dellaert
*/
// \callgraph
#pragma once
#include <list>
#include <boost/shared_ptr.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/assign/list_inserter.hpp>
#include <boost/lexical_cast.hpp>
#include <gtsam/global_includes.h>
#include <gtsam/base/FastList.h>
#include <gtsam/inference/PermutationOrdered.h>
namespace gtsam {
/**
* A BayesNet is a list of conditionals, stored in elimination order, i.e.
* leaves first, parents last. GaussianBayesNet and SymbolicBayesNet are
* defined as typedefs of this class, using GaussianConditional and
* IndexConditional as the CONDITIONAL template argument.
*
* todo: Symbolic using Index is a misnomer.
* todo: how to handle Bayes nets with an optimize function? Currently using global functions.
* \nosubgrouping
*/
template<class CONDITIONAL>
class BayesNetOrdered {
public:
typedef typename boost::shared_ptr<BayesNetOrdered<CONDITIONAL> > shared_ptr;
/** We store shared pointers to Conditional densities */
typedef typename CONDITIONAL::KeyType KeyType;
typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional;
typedef typename boost::shared_ptr<const CONDITIONAL> const_sharedConditional;
typedef typename std::list<sharedConditional> Conditionals;
typedef typename Conditionals::iterator iterator;
typedef typename Conditionals::reverse_iterator reverse_iterator;
typedef typename Conditionals::const_iterator const_iterator;
typedef typename Conditionals::const_reverse_iterator const_reverse_iterator;
protected:
/**
* Conditional densities are stored in reverse topological sort order (i.e., leaves first,
* parents last), which corresponds to the elimination ordering if so obtained,
* and is consistent with the column (block) ordering of an upper triangular matrix.
*/
Conditionals conditionals_;
public:
/// @name Standard Constructors
/// @{
/** Default constructor as an empty BayesNet */
BayesNetOrdered() {};
/** convert from a derived type */
template<class DERIVEDCONDITIONAL>
BayesNetOrdered(const BayesNetOrdered<DERIVEDCONDITIONAL>& bn) {
conditionals_.assign(bn.begin(), bn.end());
}
/** BayesNet with 1 conditional */
explicit BayesNetOrdered(const sharedConditional& conditional) { push_back(conditional); }
/// @}
/// @name Testable
/// @{
/** print */
void print(const std::string& s = "",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** check equality */
bool equals(const BayesNetOrdered& other, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
/** size is the number of nodes */
size_t size() const {
return conditionals_.size();
}
/** @return true if there are no conditionals/nodes */
bool empty() const {
return conditionals_.empty();
}
/** print statistics */
void printStats(const std::string& s = "") const;
/** save dot graph */
void saveGraph(const std::string &s, const IndexFormatter& indexFormatter =
DefaultIndexFormatter) const;
/** return keys in reverse topological sort order, i.e., elimination order */
FastList<Index> ordering() const;
/** SLOW O(n) random access to Conditional by key */
sharedConditional operator[](Index key) const;
/** return last node in ordering */
boost::shared_ptr<const CONDITIONAL> front() const { return conditionals_.front(); }
/** return last node in ordering */
boost::shared_ptr<const CONDITIONAL> back() const { return conditionals_.back(); }
/** return iterators. FD: breaks encapsulation? */
const_iterator begin() const {return conditionals_.begin();} ///<TODO: comment
const_iterator end() const {return conditionals_.end();} ///<TODO: comment
const_reverse_iterator rbegin() const {return conditionals_.rbegin();} ///<TODO: comment
const_reverse_iterator rend() const {return conditionals_.rend();} ///<TODO: comment
/** Find an iterator pointing to the conditional where the specified key
* appears as a frontal variable, or end() if no conditional contains this
* key. Running time is approximately \f$ O(n) \f$ in the number of
* conditionals in the BayesNet.
* @param key The index to find in the frontal variables of a conditional.
*/
const_iterator find(Index key) const;
/// @}
/// @name Advanced Interface
/// @{
/**
* Remove any leaf conditional. The conditional to remove is specified by
* iterator. To find the iterator pointing to the conditional containing a
* particular key, use find(), which has \f$ O(n) \f$ complexity. The
* popLeaf function by itself has \f$ O(1) \f$ complexity.
*
* If gtsam is compiled with GTSAM_EXTRA_CONSISTENCY_CHECKS defined, this
* function will check that the node is indeed a leaf, but otherwise will
* not check, because the check has \f$ O(n^2) \f$ complexity.
*
* Example 1:
\code
// Remove a leaf node with a known conditional
GaussianBayesNet gbn = ...
GaussianBayesNet::iterator leafConditional = ...
gbn.popLeaf(leafConditional);
\endcode
* Example 2:
\code
// Remove the leaf node containing variable index 14
GaussianBayesNet gbn = ...
gbn.popLeaf(gbn.find(14));
\endcode
* @param conditional The iterator pointing to the leaf conditional to remove
*/
void popLeaf(iterator conditional);
/** return last node in ordering */
sharedConditional& front() { return conditionals_.front(); }
/** return last node in ordering */
sharedConditional& back() { return conditionals_.back(); }
/** Find an iterator pointing to the conditional where the specified key
* appears as a frontal variable, or end() if no conditional contains this
* key. Running time is approximately \f$ O(n) \f$ in the number of
* conditionals in the BayesNet.
* @param key The index to find in the frontal variables of a conditional.
*/
iterator find(Index key);
/** push_back: use reverse topological sort (i.e. parents last / elimination order) */
inline void push_back(const sharedConditional& conditional) {
conditionals_.push_back(conditional);
}
/** push_front: use topological sort (i.e. parents first / reverse elimination order) */
inline void push_front(const sharedConditional& conditional) {
conditionals_.push_front(conditional);
}
/// push_back an entire Bayes net
void push_back(const BayesNetOrdered<CONDITIONAL>& bn);
/// push_front an entire Bayes net
void push_front(const BayesNetOrdered<CONDITIONAL>& bn);
/** += syntax for push_back, e.g. bayesNet += c1, c2, c3
* @param conditional The conditional to add to the back of the BayesNet
*/
boost::assign::list_inserter<boost::assign_detail::call_push_back<BayesNetOrdered<CONDITIONAL> >, sharedConditional>
operator+=(const sharedConditional& conditional) {
return boost::assign::make_list_inserter(boost::assign_detail::call_push_back<BayesNetOrdered<CONDITIONAL> >(*this))(conditional); }
/**
* pop_front: remove node at the bottom, used in marginalization
* For example P(ABC)=P(A|BC)P(B|C)P(C) becomes P(BC)=P(B|C)P(C)
*/
void pop_front() {conditionals_.pop_front();}
/** Permute the variables in the BayesNet */
void permuteWithInverse(const Permutation& inversePermutation);
iterator begin() {return conditionals_.begin();} ///<TODO: comment
iterator end() {return conditionals_.end();} ///<TODO: comment
reverse_iterator rbegin() {return conditionals_.rbegin();} ///<TODO: comment
reverse_iterator rend() {return conditionals_.rend();} ///<TODO: comment
/** saves the bayes to a text file in GraphViz format */
// void saveGraph(const std::string& s) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(conditionals_);
}
/// @}
}; // BayesNet
} // namespace gtsam
#include <gtsam/inference/BayesNetOrdered-inl.h>

72
gtsam/inference/BayesNetUnordered.h
View File

@ -1,72 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file BayesNet.h
* @brief Bayes network
* @author Frank Dellaert
* @author Richard Roberts
*/
#pragma once
#include <boost/shared_ptr.hpp>
#include <gtsam/inference/FactorGraphUnordered.h>
namespace gtsam {
/**
* A BayesNet is a tree of conditionals, stored in elimination order.
*
* todo: how to handle Bayes nets with an optimize function? Currently using global functions.
* \nosubgrouping
*/
template<class CONDITIONAL>
class BayesNetUnordered : public FactorGraphUnordered<CONDITIONAL> {
private:
typedef FactorGraphUnordered<CONDITIONAL> Base;
public:
typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional; ///< A shared pointer to a conditional
protected:
/// @name Standard Constructors
/// @{
/** Default constructor as an empty BayesNet */
BayesNetUnordered() {};
/** Construct from iterator over conditionals */
template<typename ITERATOR>
BayesNetUnordered(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {}
/// @}
public:
/// @name Testable
/// @{
/** print out graph */
void print(const std::string& s = "BayesNet",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// @}
/// @name Standard Interface
/// @{
void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
};
}

82
gtsam/inference/BayesTreeUnordered-inst.h → gtsam/inference/BayesTree-inst.h
View File

@ -20,7 +20,7 @@
#pragma once #pragma once
#include <gtsam/inference/BayesTreeUnordered.h> #include <gtsam/inference/BayesTree.h>
#include <gtsam/base/treeTraversal-inst.h> #include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
@ -34,8 +34,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::CliqueData typename BayesTree<CLIQUE>::CliqueData
BayesTreeUnordered<CLIQUE>::getCliqueData() const { BayesTree<CLIQUE>::getCliqueData() const {
CliqueData data; CliqueData data;
BOOST_FOREACH(const sharedClique& root, roots_) BOOST_FOREACH(const sharedClique& root, roots_)
getCliqueData(data, root); getCliqueData(data, root);
@ -44,7 +44,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::getCliqueData(CliqueData& data, sharedClique clique) const { void BayesTree<CLIQUE>::getCliqueData(CliqueData& data, sharedClique clique) const {
data.conditionalSizes.push_back(clique->conditional()->nrFrontals()); data.conditionalSizes.push_back(clique->conditional()->nrFrontals());
data.separatorSizes.push_back(clique->conditional()->nrParents()); data.separatorSizes.push_back(clique->conditional()->nrParents());
BOOST_FOREACH(sharedClique c, clique->children) { BOOST_FOREACH(sharedClique c, clique->children) {
@ -54,7 +54,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
size_t BayesTreeUnordered<CLIQUE>::numCachedSeparatorMarginals() const { size_t BayesTree<CLIQUE>::numCachedSeparatorMarginals() const {
size_t count = 0; size_t count = 0;
BOOST_FOREACH(const sharedClique& root, roots_) BOOST_FOREACH(const sharedClique& root, roots_)
count += root->numCachedSeparatorMarginals(); count += root->numCachedSeparatorMarginals();
@ -63,7 +63,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const { void BayesTree<CLIQUE>::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const {
if (roots_.empty()) throw std::invalid_argument("the root of Bayes tree has not been initialized!"); if (roots_.empty()) throw std::invalid_argument("the root of Bayes tree has not been initialized!");
std::ofstream of(s.c_str()); std::ofstream of(s.c_str());
of<< "digraph G{\n"; of<< "digraph G{\n";
@ -75,7 +75,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& indexFormatter, int parentnum) const { void BayesTree<CLIQUE>::saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& indexFormatter, int parentnum) const {
static int num = 0; static int num = 0;
bool first = true; bool first = true;
std::stringstream out; std::stringstream out;
@ -111,7 +111,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::CliqueStats::print(const std::string& s) const { void BayesTree<CLIQUE>::CliqueStats::print(const std::string& s) const {
std::cout << s std::cout << s
<< "avg Conditional Size: " << avgConditionalSize << std::endl << "avg Conditional Size: " << avgConditionalSize << std::endl
<< "max Conditional Size: " << maxConditionalSize << std::endl << "max Conditional Size: " << maxConditionalSize << std::endl
@ -121,8 +121,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::CliqueStats typename BayesTree<CLIQUE>::CliqueStats
BayesTreeUnordered<CLIQUE>::CliqueData::getStats() const BayesTree<CLIQUE>::CliqueData::getStats() const
{ {
CliqueStats stats; CliqueStats stats;
@ -149,7 +149,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
size_t BayesTreeUnordered<CLIQUE>::size() const { size_t BayesTree<CLIQUE>::size() const {
size_t size = 0; size_t size = 0;
BOOST_FOREACH(const sharedClique& clique, roots_) BOOST_FOREACH(const sharedClique& clique, roots_)
size += clique->treeSize(); size += clique->treeSize();
@ -158,7 +158,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) { void BayesTree<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
BOOST_FOREACH(Index j, clique->conditional()->frontals()) BOOST_FOREACH(Index j, clique->conditional()->frontals())
nodes_[j] = clique; nodes_[j] = clique;
if (parent_clique != NULL) { if (parent_clique != NULL) {
@ -171,7 +171,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::sharedClique BayesTreeUnordered<CLIQUE>::addClique( typename BayesTree<CLIQUE>::sharedClique BayesTree<CLIQUE>::addClique(
const sharedConditional& conditional, std::list<sharedClique>& child_cliques) const sharedConditional& conditional, std::list<sharedClique>& child_cliques)
{ {
sharedClique new_clique(new Clique(conditional)); sharedClique new_clique(new Clique(conditional));
@ -186,7 +186,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
template<class FACTOR, class CLIQUE> template<class FACTOR, class CLIQUE>
int _pushClique(FactorGraphUnordered<FACTOR>& fg, const boost::shared_ptr<CLIQUE>& clique) { int _pushClique(FactorGraph<FACTOR>& fg, const boost::shared_ptr<CLIQUE>& clique) {
fg.push_back(clique->conditional_); fg.push_back(clique->conditional_);
return 0; return 0;
} }
@ -194,7 +194,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::addFactorsToGraph(FactorGraphUnordered<FactorType>& graph) const void BayesTree<CLIQUE>::addFactorsToGraph(FactorGraph<FactorType>& graph) const
{ {
// Traverse the BayesTree and add all conditionals to this graph // Traverse the BayesTree and add all conditionals to this graph
int data = 0; // Unused int data = 0; // Unused
@ -203,7 +203,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
BayesTreeUnordered<CLIQUE>::BayesTreeUnordered(const This& other) { BayesTree<CLIQUE>::BayesTree(const This& other) {
*this = other; *this = other;
} }
@ -224,7 +224,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
BayesTreeUnordered<CLIQUE>& BayesTreeUnordered<CLIQUE>::operator=(const This& other) { BayesTree<CLIQUE>& BayesTree<CLIQUE>::operator=(const This& other) {
this->clear(); this->clear();
boost::shared_ptr<Clique> rootContainer = boost::make_shared<Clique>(); boost::shared_ptr<Clique> rootContainer = boost::make_shared<Clique>();
treeTraversal::DepthFirstForest(other, rootContainer, BayesTreeCloneForestVisitorPre<Clique>); treeTraversal::DepthFirstForest(other, rootContainer, BayesTreeCloneForestVisitorPre<Clique>);
@ -237,7 +237,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::print(const std::string& s, const KeyFormatter& keyFormatter) const { void BayesTree<CLIQUE>::print(const std::string& s, const KeyFormatter& keyFormatter) const {
std::cout << s << ": cliques: " << size() << ", variables: " << nodes_.size() << std::endl; std::cout << s << ": cliques: " << size() << ", variables: " << nodes_.size() << std::endl;
treeTraversal::PrintForest(*this, s, keyFormatter); treeTraversal::PrintForest(*this, s, keyFormatter);
} }
@ -246,8 +246,8 @@ namespace gtsam {
// binary predicate to test equality of a pair for use in equals // binary predicate to test equality of a pair for use in equals
template<class CLIQUE> template<class CLIQUE>
bool check_sharedCliques( bool check_sharedCliques(
const std::pair<Key, typename BayesTreeUnordered<CLIQUE>::sharedClique>& v1, const std::pair<Key, typename BayesTree<CLIQUE>::sharedClique>& v1,
const std::pair<Key, typename BayesTreeUnordered<CLIQUE>::sharedClique>& v2 const std::pair<Key, typename BayesTree<CLIQUE>::sharedClique>& v2
) { ) {
return v1.first == v2.first && return v1.first == v2.first &&
((!v1.second && !v2.second) || (v1.second && v2.second && v1.second->equals(*v2.second))); ((!v1.second && !v2.second) || (v1.second && v2.second && v1.second->equals(*v2.second)));
@ -255,7 +255,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
bool BayesTreeUnordered<CLIQUE>::equals(const BayesTreeUnordered<CLIQUE>& other, double tol) const { bool BayesTree<CLIQUE>::equals(const BayesTree<CLIQUE>& other, double tol) const {
return size()==other.size() && return size()==other.size() &&
std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CLIQUE>); std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CLIQUE>);
} }
@ -263,7 +263,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
template<class CONTAINER> template<class CONTAINER>
Key BayesTreeUnordered<CLIQUE>::findParentClique(const CONTAINER& parents) const { Key BayesTree<CLIQUE>::findParentClique(const CONTAINER& parents) const {
typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end()); typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end());
assert(lowestOrderedParent != parents.end()); assert(lowestOrderedParent != parents.end());
return *lowestOrderedParent; return *lowestOrderedParent;
@ -271,18 +271,18 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::fillNodesIndex(const sharedClique& subtree) { void BayesTree<CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
// Add each frontal variable of this root node // Add each frontal variable of this root node
BOOST_FOREACH(const Key& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; } BOOST_FOREACH(const Key& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; }
// Fill index for each child // Fill index for each child
typedef typename BayesTreeUnordered<CLIQUE>::sharedClique sharedClique; typedef typename BayesTree<CLIQUE>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& child, subtree->children) { BOOST_FOREACH(const sharedClique& child, subtree->children) {
fillNodesIndex(child); } fillNodesIndex(child); }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::insertRoot(const sharedClique& subtree) { void BayesTree<CLIQUE>::insertRoot(const sharedClique& subtree) {
roots_.push_back(subtree); // Add to roots roots_.push_back(subtree); // Add to roots
fillNodesIndex(subtree); // Populate nodes index fillNodesIndex(subtree); // Populate nodes index
} }
@ -291,8 +291,8 @@ namespace gtsam {
// First finds clique marginal then marginalizes that // First finds clique marginal then marginalizes that
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::sharedConditional typename BayesTree<CLIQUE>::sharedConditional
BayesTreeUnordered<CLIQUE>::marginalFactor(Key j, const Eliminate& function) const BayesTree<CLIQUE>::marginalFactor(Key j, const Eliminate& function) const
{ {
gttic(BayesTree_marginalFactor); gttic(BayesTree_marginalFactor);
@ -304,7 +304,7 @@ namespace gtsam {
// Now, marginalize out everything that is not variable j // Now, marginalize out everything that is not variable j
BayesNetType marginalBN = *cliqueMarginal.marginalMultifrontalBayesNet( BayesNetType marginalBN = *cliqueMarginal.marginalMultifrontalBayesNet(
OrderingUnordered(cref_list_of<1,Key>(j)), boost::none, function); Ordering(cref_list_of<1,Key>(j)), boost::none, function);
// The Bayes net should contain only one conditional for variable j, so return it // The Bayes net should contain only one conditional for variable j, so return it
return marginalBN.front(); return marginalBN.front();
@ -314,8 +314,8 @@ namespace gtsam {
// Find two cliques, their joint, then marginalizes // Find two cliques, their joint, then marginalizes
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::sharedFactorGraph typename BayesTree<CLIQUE>::sharedFactorGraph
BayesTreeUnordered<CLIQUE>::joint(Key j1, Key j2, const Eliminate& function) const BayesTree<CLIQUE>::joint(Key j1, Key j2, const Eliminate& function) const
{ {
gttic(BayesTree_joint); gttic(BayesTree_joint);
return boost::make_shared<FactorGraphType>(*jointBayesNet(j1, j2, function)); return boost::make_shared<FactorGraphType>(*jointBayesNet(j1, j2, function));
@ -323,8 +323,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::sharedBayesNet typename BayesTree<CLIQUE>::sharedBayesNet
BayesTreeUnordered<CLIQUE>::jointBayesNet(Key j1, Key j2, const Eliminate& function) const BayesTree<CLIQUE>::jointBayesNet(Key j1, Key j2, const Eliminate& function) const
{ {
gttic(BayesTree_jointBayesNet); gttic(BayesTree_jointBayesNet);
// get clique C1 and C2 // get clique C1 and C2
@ -385,7 +385,7 @@ namespace gtsam {
// Factor into C1\B | B. // Factor into C1\B | B.
sharedFactorGraph temp_remaining; sharedFactorGraph temp_remaining;
boost::tie(p_C1_B, temp_remaining) = boost::tie(p_C1_B, temp_remaining) =
FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(OrderingUnordered(C1_minus_B), function); FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function);
} }
shared_ptr p_C2_B; { shared_ptr p_C2_B; {
std::vector<Index> C2_minus_B; { std::vector<Index> C2_minus_B; {
@ -397,7 +397,7 @@ namespace gtsam {
// Factor into C2\B | B. // Factor into C2\B | B.
sharedFactorGraph temp_remaining; sharedFactorGraph temp_remaining;
boost::tie(p_C2_B, temp_remaining) = boost::tie(p_C2_B, temp_remaining) =
FactorGraphType(p_C2_Bred).eliminatePartialMultifrontal(OrderingUnordered(C2_minus_B), function); FactorGraphType(p_C2_Bred).eliminatePartialMultifrontal(Ordering(C2_minus_B), function);
} }
gttoc(Full_root_factoring); gttoc(Full_root_factoring);
@ -413,12 +413,12 @@ namespace gtsam {
p_BC1C2 += C2->conditional(); p_BC1C2 += C2->conditional();
// now, marginalize out everything that is not variable j1 or j2 // now, marginalize out everything that is not variable j1 or j2
return p_BC1C2.marginalMultifrontalBayesNet(OrderingUnordered(cref_list_of<2,Key>(j1)(j2)), boost::none, function); return p_BC1C2.marginalMultifrontalBayesNet(Ordering(cref_list_of<2,Key>(j1)(j2)), boost::none, function);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::clear() { void BayesTree<CLIQUE>::clear() {
// Remove all nodes and clear the root pointer // Remove all nodes and clear the root pointer
nodes_.clear(); nodes_.clear();
roots_.clear(); roots_.clear();
@ -426,7 +426,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::deleteCachedShortcuts() { void BayesTree<CLIQUE>::deleteCachedShortcuts() {
BOOST_FOREACH(const sharedClique& root, roots_) { BOOST_FOREACH(const sharedClique& root, roots_) {
root->deleteCachedShortcuts(); root->deleteCachedShortcuts();
} }
@ -434,7 +434,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::removeClique(sharedClique clique) void BayesTree<CLIQUE>::removeClique(sharedClique clique)
{ {
if (clique->isRoot()) { if (clique->isRoot()) {
std::vector<sharedClique>::iterator root = std::find(roots_.begin(), roots_.end(), clique); std::vector<sharedClique>::iterator root = std::find(roots_.begin(), roots_.end(), clique);
@ -458,7 +458,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans) void BayesTree<CLIQUE>::removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans)
{ {
// base case is NULL, if so we do nothing and return empties above // base case is NULL, if so we do nothing and return empties above
if (clique!=NULL) { if (clique!=NULL) {
@ -483,7 +483,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::removeTop(const std::vector<Key>& keys, BayesNetType& bn, Cliques& orphans) void BayesTree<CLIQUE>::removeTop(const std::vector<Key>& keys, BayesNetType& bn, Cliques& orphans)
{ {
// process each key of the new factor // process each key of the new factor
BOOST_FOREACH(const Key& j, keys) BOOST_FOREACH(const Key& j, keys)
@ -505,7 +505,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::Cliques BayesTreeUnordered<CLIQUE>::removeSubtree( typename BayesTree<CLIQUE>::Cliques BayesTree<CLIQUE>::removeSubtree(
const sharedClique& subtree) const sharedClique& subtree)
{ {
// Result clique list // Result clique list

677
gtsam/inference/BayesTree.h
View File

@ -1,418 +1,259 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file BayesTree.h * @file BayesTree.h
* @brief Bayes Tree is a tree of cliques of a Bayes Chain * @brief Bayes Tree is a tree of cliques of a Bayes Chain
* @author Frank Dellaert * @author Frank Dellaert
*/ */
// \callgraph // \callgraph
#pragma once #pragma once
#include <vector> #include <vector>
#include <stdexcept> #include <string>
#include <deque>
#include <boost/function.hpp> #include <gtsam/base/types.h>
#include <boost/shared_ptr.hpp> #include <gtsam/base/FastList.h>
#include <boost/make_shared.hpp> #include <gtsam/base/FastMap.h>
#include <boost/lexical_cast.hpp>
namespace gtsam {
#include <gtsam/global_includes.h>
#include <gtsam/base/FastList.h> // Forward declarations
#include <gtsam/inference/FactorGraph.h> template<class FACTOR> class FactorGraph;
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/BayesTreeCliqueBase.h> /**
#include <gtsam/inference/IndexConditional.h> * Bayes tree
#include <gtsam/linear/VectorValues.h> * @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain,
* which could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional.
namespace gtsam { * @tparam CLIQUE The type of the clique data structure, defaults to BayesTreeClique, normally do not change this
* as it is only used when developing special versions of BayesTree, e.g. for ISAM2.
// Forward declaration of BayesTreeClique which is defined below BayesTree in this file *
template<class CONDITIONAL> struct BayesTreeClique; * \addtogroup Multifrontal
* \nosubgrouping
/** */
* Bayes tree template<class CLIQUE>
* @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain, class BayesTree
* which could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional. {
* @tparam CLIQUE The type of the clique data structure, defaults to BayesTreeClique, normally do not change this protected:
* as it is only used when developing special versions of BayesTree, e.g. for ISAM2. typedef BayesTree<CLIQUE> This;
* typedef boost::shared_ptr<This> shared_ptr;
* \addtogroup Multifrontal
* \nosubgrouping public:
*/ typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
template<class CONDITIONAL, class CLIQUE=BayesTreeClique<CONDITIONAL> > typedef boost::shared_ptr<Clique> sharedClique; ///< Shared pointer to a clique
class BayesTree { typedef Clique Node; ///< Synonym for Clique (TODO: remove)
typedef sharedClique sharedNode; ///< Synonym for sharedClique (TODO: remove)
public: typedef typename CLIQUE::ConditionalType ConditionalType;
typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef BayesTree<CONDITIONAL, CLIQUE> This; typedef typename CLIQUE::BayesNetType BayesNetType;
typedef boost::shared_ptr<BayesTree<CONDITIONAL, CLIQUE> > shared_ptr; typedef boost::shared_ptr<BayesNetType> sharedBayesNet;
typedef boost::shared_ptr<CONDITIONAL> sharedConditional; typedef typename CLIQUE::FactorType FactorType;
typedef boost::shared_ptr<BayesNet<CONDITIONAL> > sharedBayesNet; typedef boost::shared_ptr<FactorType> sharedFactor;
typedef CONDITIONAL ConditionalType; typedef typename CLIQUE::FactorGraphType FactorGraphType;
typedef typename CONDITIONAL::FactorType FactorType; typedef boost::shared_ptr<FactorGraphType> sharedFactorGraph;
typedef typename FactorGraph<FactorType>::Eliminate Eliminate; typedef typename FactorGraphType::Eliminate Eliminate;
typedef typename CLIQUE::EliminationTraits EliminationTraits;
typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
/** A convenience class for a list of shared cliques */
// typedef for shared pointers to cliques typedef FastList<sharedClique> Cliques;
typedef boost::shared_ptr<Clique> sharedClique;
/** clique statistics */
// A convenience class for a list of shared cliques struct CliqueStats {
struct Cliques : public FastList<sharedClique> { double avgConditionalSize;
void print(const std::string& s = "Cliques", std::size_t maxConditionalSize;
const IndexFormatter& indexFormatter = DefaultIndexFormatter) const; double avgSeparatorSize;
bool equals(const Cliques& other, double tol = 1e-9) const; std::size_t maxSeparatorSize;
}; void print(const std::string& s = "") const ;
};
/** clique statistics */
struct CliqueStats { /** store all the sizes */
double avgConditionalSize; struct CliqueData {
std::size_t maxConditionalSize; std::vector<std::size_t> conditionalSizes;
double avgSeparatorSize; std::vector<std::size_t> separatorSizes;
std::size_t maxSeparatorSize; CliqueStats getStats() const;
void print(const std::string& s = "") const ; };
};
/** Map from keys to Clique */
/** store all the sizes */ typedef FastMap<Key, sharedClique> Nodes;
struct CliqueData {
std::vector<std::size_t> conditionalSizes; protected:
std::vector<std::size_t> separatorSizes;
CliqueStats getStats() const; /** Map from indices to Clique */
}; Nodes nodes_;
/** Map from indices to Clique */ /** Root cliques */
typedef std::vector<sharedClique> Nodes; std::vector<sharedClique> roots_;
protected: /// @name Standard Constructors
/// @{
/** Map from indices to Clique */
Nodes nodes_; /** Create an empty Bayes Tree */
BayesTree() {}
/** Root clique */
sharedClique root_; /** Copy constructor */
BayesTree(const This& other);
public:
/// @}
/// @name Standard Constructors
/// @{ /** Assignment operator */
This& operator=(const This& other);
/** Create an empty Bayes Tree */
BayesTree() {} /// @name Testable
/// @{
/** Create a Bayes Tree from a Bayes Net (requires CONDITIONAL is IndexConditional *or* CONDITIONAL::Combine) */
explicit BayesTree(const BayesNet<CONDITIONAL>& bayesNet); /** check equality */
bool equals(const This& other, double tol = 1e-9) const;
/** Copy constructor */
BayesTree(const This& other); public:
/** print */
/** Assignment operator */ void print(const std::string& s = "",
This& operator=(const This& other); const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @}
/// @}
/// @name Advanced Constructors /// @name Standard Interface
/// @{ /// @{
/** /** number of cliques */
* Create a Bayes Tree from a Bayes Net and some subtrees. The Bayes net corresponds to the size_t size() const;
* new root clique and the subtrees are connected to the root clique.
*/ /** Check if there are any cliques in the tree */
BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL,CLIQUE> > subtrees); inline bool empty() const {
return nodes_.empty();
/** Destructor */ }
virtual ~BayesTree() {}
/** return nodes */
/// @} const Nodes& nodes() const { return nodes_; }
/// @name Testable
/// @{ /** Access node by variable */
const sharedNode operator[](Key j) const { return nodes_.at(j); }
/** check equality */
bool equals(const BayesTree<CONDITIONAL,CLIQUE>& other, double tol = 1e-9) const; /** return root cliques */
const std::vector<sharedClique>& roots() const { return roots_; }
/** print */
void print(const std::string& s = "", /** alternate syntax for matlab: find the clique that contains the variable with Index j */
const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const; const sharedClique& clique(Key j) const {
Nodes::const_iterator c = nodes_.find(j);
/// @} if(c == nodes_.end())
/// @name Standard Interface throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree");
/// @{ else
return c->second;
/** }
* Find parent clique of a conditional. It will look at all parents and
* return the one with the lowest index in the ordering. /** Gather data on all cliques */
*/ CliqueData getCliqueData() const;
template<class CONTAINER>
Index findParentClique(const CONTAINER& parents) const; /** Collect number of cliques with cached separator marginals */
size_t numCachedSeparatorMarginals() const;
/** number of cliques */
inline size_t size() const { /** Return marginal on any variable. Note that this actually returns a conditional, for which a
if(root_) * solution may be directly obtained by calling .solve() on the returned object.
return root_->treeSize(); * Alternatively, it may be directly used as its factor base class. For example, for Gaussian
else * systems, this returns a GaussianConditional, which inherits from JacobianFactor and
return 0; * GaussianFactor. */
} sharedConditional marginalFactor(Key j, const Eliminate& function = EliminationTraits::DefaultEliminate) const;
/** number of nodes */ /**
inline size_t nrNodes() const { return nodes_.size(); } * return joint on two variables
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
/** Check if there are any cliques in the tree */ */
inline bool empty() const { sharedFactorGraph joint(Index j1, Index j2, const Eliminate& function = EliminationTraits::DefaultEliminate) const;
return nodes_.empty();
} /**
* return joint on two variables as a BayesNet
/** return nodes */ * Limitation: can only calculate joint if cliques are disjoint or one of them is root
const Nodes& nodes() const { return nodes_; } */
sharedBayesNet jointBayesNet(Index j1, Index j2, const Eliminate& function = EliminationTraits::DefaultEliminate) const;
/** return root clique */
const sharedClique& root() const { return root_; } /**
* Read only with side effects
/** find the clique that contains the variable with Index j */ */
inline sharedClique operator[](Index j) const {
return nodes_.at(j); /** saves the Tree to a text file in GraphViz format */
} void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** alternate syntax for matlab: find the clique that contains the variable with Index j */ /// @}
inline sharedClique clique(Index j) const { /// @name Advanced Interface
return nodes_.at(j); /// @{
}
/**
/** Gather data on all cliques */ * Find parent clique of a conditional. It will look at all parents and
CliqueData getCliqueData() const; * return the one with the lowest index in the ordering.
*/
/** Collect number of cliques with cached separator marginals */ template<class CONTAINER>
size_t numCachedSeparatorMarginals() const; Index findParentClique(const CONTAINER& parents) const;
/** return marginal on any variable */ /** Remove all nodes */
typename FactorType::shared_ptr marginalFactor(Index j, Eliminate function) const; void clear();
/** /** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */
* return marginal on any variable, as a Bayes Net void deleteCachedShortcuts();
* NOTE: this function calls marginal, and then eliminates it into a Bayes Net
* This is more expensive than the above function /**
*/ * Remove path from clique to root and return that path as factors
typename BayesNet<CONDITIONAL>::shared_ptr marginalBayesNet(Index j, Eliminate function) const; * plus a list of orphaned subtree roots. Used in removeTop below.
*/
/** void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans);
* return joint on two variables
* Limitation: can only calculate joint if cliques are disjoint or one of them is root /**
*/ * Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
typename FactorGraph<FactorType>::shared_ptr joint(Index j1, Index j2, Eliminate function) const; * Factors and orphans are added to the in/out arguments.
*/
/** void removeTop(const std::vector<Key>& keys, BayesNetType& bn, Cliques& orphans);
* return joint on two variables as a BayesNet
* Limitation: can only calculate joint if cliques are disjoint or one of them is root /**
*/ * Remove the requested subtree. */
typename BayesNet<CONDITIONAL>::shared_ptr jointBayesNet(Index j1, Index j2, Eliminate function) const; Cliques removeSubtree(const sharedClique& subtree);
/** /** Insert a new subtree with known parent clique. This function does not check that the
* Read only with side effects * specified parent is the correct parent. This function updates all of the internal data
*/ * structures associated with adding a subtree, such as populating the nodes index. */
void insertRoot(const sharedClique& subtree);
/** saves the Tree to a text file in GraphViz format */
void saveGraph(const std::string& s, const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const; /** add a clique (top down) */
void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());
/// @}
/// @name Advanced Interface /** Add all cliques in this BayesTree to the specified factor graph */
/// @{ void addFactorsToGraph(FactorGraph<FactorType>& graph) const;
/** Access the root clique (non-const version) */ protected:
sharedClique& root() { return root_; }
/** private helper method for saving the Tree to a text file in GraphViz format */
/** Access the nodes (non-cost version) */ void saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter,
Nodes& nodes() { return nodes_; } int parentnum = 0) const;
/** Remove all nodes */ /** Gather data on a single clique */
void clear(); void getCliqueData(CliqueData& stats, sharedClique clique) const;
/** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */ /** remove a clique: warning, can result in a forest */
void deleteCachedShortcuts() { void removeClique(sharedClique clique);
root_->deleteCachedShortcuts();
} /** add a clique (bottom up) */
sharedClique addClique(const sharedConditional& conditional, std::list<sharedClique>& child_cliques);
/** Apply a permutation to the full tree - also updates the nodes structure */
void permuteWithInverse(const Permutation& inversePermutation); /** Fill the nodes index for a subtree */
void fillNodesIndex(const sharedClique& subtree);
/**
* Remove path from clique to root and return that path as factors private:
* plus a list of orphaned subtree roots. Used in removeTop below. /** Serialization function */
*/ friend class boost::serialization::access;
void removePath(sharedClique clique, BayesNet<CONDITIONAL>& bn, Cliques& orphans); template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
/** ar & BOOST_SERIALIZATION_NVP(nodes_);
* Given a list of indices, turn "contaminated" part of the tree back into a factor graph. ar & BOOST_SERIALIZATION_NVP(root_);
* Factors and orphans are added to the in/out arguments. }
*/
template<class CONTAINER> /// @}
void removeTop(const CONTAINER& indices, BayesNet<CONDITIONAL>& bn, Cliques& orphans);
}; // BayesTree
/**
* Remove the requested subtree. */ } /// namespace gtsam
Cliques removeSubtree(const sharedClique& subtree);
/**
* Hang a new subtree off of the existing tree. This finds the appropriate
* parent clique for the subtree (which may be the root), and updates the
* nodes index with the new cliques in the subtree. None of the frontal
* variables in the subtree may appear in the separators of the existing
* BayesTree.
*/
void insert(const sharedClique& subtree);
/** Insert a new conditional
* This function only applies for Symbolic case with IndexCondtional,
* We make it static so that it won't be compiled in GaussianConditional case.
* */
static void insert(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional);
/**
* Insert a new clique corresponding to the given Bayes net.
* It is the caller's responsibility to decide whether the given Bayes net is a valid clique,
* i.e. all the variables (frontal and separator) are connected
*/
sharedClique insert(const sharedConditional& clique,
std::list<sharedClique>& children, bool isRootClique = false);
/**
* Create a clone of this object as a shared pointer
* Necessary for inheritance in matlab interface
*/
virtual shared_ptr clone() const {
return shared_ptr(new This(*this));
}
protected:
/** private helper method for saving the Tree to a text file in GraphViz format */
void saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter,
int parentnum = 0) const;
/** Gather data on a single clique */
void getCliqueData(CliqueData& stats, sharedClique clique) const;
/** remove a clique: warning, can result in a forest */
void removeClique(sharedClique clique);
/** add a clique (top down) */
sharedClique addClique(const sharedConditional& conditional, const sharedClique& parent_clique = sharedClique());
/** add a clique (top down) */
void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());
/** add a clique (bottom up) */
sharedClique addClique(const sharedConditional& conditional, std::list<sharedClique>& child_cliques);
/**
* Add a conditional to the front of a clique, i.e. a conditional whose
* parents are already in the clique or its separators. This function does
* not check for this condition, it just updates the data structures.
*/
static void addToCliqueFront(BayesTree<CONDITIONAL,CLIQUE>& bayesTree,
const sharedConditional& conditional, const sharedClique& clique);
/** Fill the nodes index for a subtree */
void fillNodesIndex(const sharedClique& subtree);
/** Helper function to build a non-symbolic tree (e.g. Gaussian) using a
* symbolic tree, used in the BT(BN) constructor.
*/
void recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditional> >& symbolic,
const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals,
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& parent);
private:
/** deep copy to another tree */
void cloneTo(This& newTree) const;
/** deep copy to another tree */
void cloneTo(This& newTree, const sharedClique& subtree, const sharedClique& parent) const;
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(nodes_);
ar & BOOST_SERIALIZATION_NVP(root_);
}
/// @}
}; // BayesTree
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void _BayesTree_dim_adder(
std::vector<size_t>& dims,
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) {
if(clique) {
// Add dims from this clique
for(typename CONDITIONAL::const_iterator it = (*clique)->beginFrontals(); it != (*clique)->endFrontals(); ++it)
dims[*it] = (*clique)->dim(it);
// Traverse children
typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& child, clique->children()) {
_BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dims, child);
}
}
}
/* ************************************************************************* */
template<class CONDITIONAL,class CLIQUE>
boost::shared_ptr<VectorValues> allocateVectorValues(const BayesTree<CONDITIONAL,CLIQUE>& bt) {
std::vector<size_t> dimensions(bt.nodes().size(), 0);
_BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dimensions, bt.root());
return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
}
/* ************************************************************************* */
/**
* A Clique in the tree is an incomplete Bayes net: the variables
* in the Bayes net are the frontal nodes, and the variables conditioned
* on are the separator. We also have pointers up and down the tree.
*
* Since our Conditional class already handles multiple frontal variables,
* this Clique contains exactly 1 conditional.
*
* This is the default clique type in a BayesTree, but some algorithms, like
* iSAM2 (see ISAM2Clique), use a different clique type in order to store
* extra data along with the clique.
*/
template<class CONDITIONAL>
struct BayesTreeClique : public BayesTreeCliqueBase<BayesTreeClique<CONDITIONAL>, CONDITIONAL> {
public:
typedef CONDITIONAL ConditionalType;
typedef BayesTreeClique<CONDITIONAL> This;
typedef BayesTreeCliqueBase<This, CONDITIONAL> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
BayesTreeClique() {}
BayesTreeClique(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {}
BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
};
} /// namespace gtsam
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/inference/BayesTreeCliqueBase-inl.h>

52
gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h → gtsam/inference/BayesTreeCliqueBase-inst.h
View File

@ -16,7 +16,7 @@
#pragma once #pragma once
#include <gtsam/inference/BayesTreeCliqueBaseUnordered.h> #include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
@ -24,7 +24,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH> template<class DERIVED, class FACTORGRAPH>
bool BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::equals( bool BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::equals(
const DERIVED& other, double tol = 1e-9) const const DERIVED& other, double tol = 1e-9) const
{ {
return (!conditional_ && !other.conditional()) return (!conditional_ && !other.conditional())
@ -34,7 +34,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH> template<class DERIVED, class FACTORGRAPH>
std::vector<Key> std::vector<Key>
BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const
{ {
FastSet<Key> p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); FastSet<Key> p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end()); FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
@ -46,7 +46,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH> template<class DERIVED, class FACTORGRAPH>
std::vector<Key> BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::shortcut_indices( std::vector<Key> BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut_indices(
const derived_ptr& B, const FactorGraphType& p_Cp_B) const const derived_ptr& B, const FactorGraphType& p_Cp_B) const
{ {
gttic(shortcut_indices); gttic(shortcut_indices);
@ -65,7 +65,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH> template<class DERIVED, class FACTORGRAPH>
void BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::print( void BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::print(
const std::string& s, const KeyFormatter& keyFormatter) const const std::string& s, const KeyFormatter& keyFormatter) const
{ {
conditional_->print(s, keyFormatter); conditional_->print(s, keyFormatter);
@ -73,7 +73,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH> template<class DERIVED, class FACTORGRAPH>
size_t BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::treeSize() const { size_t BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::treeSize() const {
size_t size = 1; size_t size = 1;
BOOST_FOREACH(const derived_ptr& child, children) BOOST_FOREACH(const derived_ptr& child, children)
size += child->treeSize(); size += child->treeSize();
@ -82,7 +82,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH> template<class DERIVED, class FACTORGRAPH>
size_t BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::numCachedSeparatorMarginals() const size_t BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::numCachedSeparatorMarginals() const
{ {
if (!cachedSeparatorMarginal_) if (!cachedSeparatorMarginal_)
return 0; return 0;
@ -100,10 +100,10 @@ namespace gtsam {
// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp 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
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH> template<class DERIVED, class FACTORGRAPH>
typename BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::BayesNetType typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::BayesNetType
BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::shortcut(const derived_ptr& B, Eliminate function) const BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut(const derived_ptr& B, Eliminate function) const
{ {
gttic(BayesTreeCliqueBaseUnordered_shortcut); gttic(BayesTreeCliqueBase_shortcut);
// We only calculate the shortcut when this clique is not B // We only calculate the shortcut when this clique is not B
// and when the S\B is not empty // and when the S\B is not empty
std::vector<Key> S_setminus_B = separator_setminus_B(B); std::vector<Key> S_setminus_B = separator_setminus_B(B);
@ -111,9 +111,9 @@ namespace gtsam {
{ {
// Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
derived_ptr parent(parent_.lock()); derived_ptr parent(parent_.lock());
gttoc(BayesTreeCliqueBaseUnordered_shortcut); gttoc(BayesTreeCliqueBase_shortcut);
FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B) FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B)
gttic(BayesTreeCliqueBaseUnordered_shortcut); gttic(BayesTreeCliqueBase_shortcut);
p_Cp_B += parent->conditional_; // P(Fp|Sp) p_Cp_B += parent->conditional_; // P(Fp|Sp)
// Determine the variables we want to keepSet, S union B // Determine the variables we want to keepSet, S union B
@ -121,7 +121,7 @@ namespace gtsam {
// Marginalize out everything except S union B // Marginalize out everything except S union B
BayesNetType result = *p_Cp_B.marginalMultifrontalBayesNet( BayesNetType result = *p_Cp_B.marginalMultifrontalBayesNet(
OrderingUnordered(keep), boost::none, function); Ordering(keep), boost::none, function);
// Finally, we only want to have S\B variables in the Bayes net, so // Finally, we only want to have S\B variables in the Bayes net, so
size_t nrFrontals = S_setminus_B.size(); size_t nrFrontals = S_setminus_B.size();
@ -140,14 +140,14 @@ namespace gtsam {
// P(C) = P(F|S) P(S) // P(C) = P(F|S) P(S)
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH> template<class DERIVED, class FACTORGRAPH>
typename BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::FactorGraphType typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::separatorMarginal(Eliminate function) const BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separatorMarginal(Eliminate function) const
{ {
gttic(BayesTreeCliqueBaseUnordered_separatorMarginal); gttic(BayesTreeCliqueBase_separatorMarginal);
// Check if the Separator marginal was already calculated // Check if the Separator marginal was already calculated
if (!cachedSeparatorMarginal_) if (!cachedSeparatorMarginal_)
{ {
gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
// If this is the root, there is no separator // If this is the root, there is no separator
if (parent_.expired() /*(if we're the root)*/) if (parent_.expired() /*(if we're the root)*/)
{ {
@ -160,17 +160,17 @@ namespace gtsam {
// Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp) // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
// initialize P(Cp) with the parent separator marginal // initialize P(Cp) with the parent separator marginal
derived_ptr parent(parent_.lock()); derived_ptr parent(parent_.lock());
gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); // Flatten recursion in timing outline gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss); // Flatten recursion in timing outline
gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal); gttoc(BayesTreeCliqueBase_separatorMarginal);
FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp) FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp)
gttic(BayesTreeCliqueBaseUnordered_separatorMarginal); gttic(BayesTreeCliqueBase_separatorMarginal);
gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
// now add the parent conditional // now add the parent conditional
p_Cp += parent->conditional_; // P(Fp|Sp) p_Cp += parent->conditional_; // P(Fp|Sp)
// The variables we want to keepSet are exactly the ones in S // The variables we want to keepSet are exactly the ones in S
std::vector<Key> indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); std::vector<Key> indicesS(this->conditional()->beginParents(), this->conditional()->endParents());
cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(OrderingUnordered(indicesS), boost::none, function); cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), boost::none, function);
} }
} }
@ -183,10 +183,10 @@ namespace gtsam {
// P(C) = P(F|S) P(S) // P(C) = P(F|S) P(S)
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH> template<class DERIVED, class FACTORGRAPH>
typename BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::FactorGraphType typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::marginal2(Eliminate function) const BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::marginal2(Eliminate function) const
{ {
gttic(BayesTreeCliqueBaseUnordered_marginal2); gttic(BayesTreeCliqueBase_marginal2);
// initialize with separator marginal P(S) // initialize with separator marginal P(S)
FactorGraphType p_C = this->separatorMarginal(function); FactorGraphType p_C = this->separatorMarginal(function);
// add the conditional P(F|S) // add the conditional P(F|S)
@ -196,7 +196,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH> template<class DERIVED, class FACTORGRAPH>
void BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::deleteCachedShortcuts() { void BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::deleteCachedShortcuts() {
// When a shortcut is requested, all of the shortcuts between it and the // When a shortcut is requested, all of the shortcuts between it and the
// root are also generated. So, if this clique's cached shortcut is set, // root are also generated. So, if this clique's cached shortcut is set,

435
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -1,273 +1,162 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file BayesTreeCliqueBase.h * @file BayesTreeCliqueBase.h
* @brief Base class for cliques of a BayesTree * @brief Base class for cliques of a BayesTree
* @author Richard Roberts and Frank Dellaert * @author Richard Roberts and Frank Dellaert
*/ */
#pragma once #pragma once
#include <boost/shared_ptr.hpp> #include <gtsam/base/types.h>
#include <boost/make_shared.hpp>
#include <boost/weak_ptr.hpp> namespace gtsam {
#include <gtsam/global_includes.h> // Forward declarations
#include <gtsam/inference/FactorGraph.h> template<class CLIQUE> class BayesTree;
#include <gtsam/inference/BayesNet.h> template<class GRAPH> struct EliminationTraits;
namespace gtsam { /**
template<class CONDITIONAL, class CLIQUE> class BayesTree; * This is the base class for BayesTree cliques. The default and standard derived type is
} * BayesTreeClique, but some algorithms, like iSAM2, use a different clique type in order to store
* extra data along with the clique.
namespace gtsam { *
* This class is templated on the derived class (i.e. the curiously recursive template pattern).
/** * The advantage of this over using virtual classes is that it avoids the need for casting to get
* This is the base class for BayesTree cliques. The default and standard * the derived type. This is possible because all cliques in a BayesTree are the same type - if
* derived type is BayesTreeClique, but some algorithms, like iSAM2, use a * they were not then we'd need a virtual class.
* different clique type in order to store extra data along with the clique. *
* * @tparam DERIVED The derived clique type.
* This class is templated on the derived class (i.e. the curiously recursive * @tparam CONDITIONAL The conditional type.
* template pattern). The advantage of this over using virtual classes is * \nosubgrouping */
* that it avoids the need for casting to get the derived type. This is template<class DERIVED, class FACTORGRAPH>
* possible because all cliques in a BayesTree are the same type - if they class BayesTreeCliqueBase
* were not then we'd need a virtual class. {
* private:
* @tparam DERIVED The derived clique type. typedef BayesTreeCliqueBase<DERIVED, FACTORGRAPH> This;
* @tparam CONDITIONAL The conditional type. typedef DERIVED DerivedType;
* \nosubgrouping typedef EliminationTraits<FACTORGRAPH> EliminationTraits;
*/ typedef boost::shared_ptr<This> shared_ptr;
template<class DERIVED, class CONDITIONAL> typedef boost::weak_ptr<This> weak_ptr;
struct BayesTreeCliqueBase { typedef boost::shared_ptr<DerivedType> derived_ptr;
typedef boost::weak_ptr<DerivedType> derived_weak_ptr;
public:
typedef BayesTreeCliqueBase<DERIVED, CONDITIONAL> This; public:
typedef DERIVED DerivedType; typedef FACTORGRAPH FactorGraphType;
typedef CONDITIONAL ConditionalType; typedef typename EliminationTraits::BayesNetType BayesNetType;
typedef boost::shared_ptr<ConditionalType> sharedConditional; typedef typename BayesNetType::ConditionalType ConditionalType;
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef boost::weak_ptr<This> weak_ptr; typedef typename FactorGraphType::FactorType FactorType;
typedef boost::shared_ptr<DerivedType> derived_ptr; typedef typename FactorGraphType::Eliminate Eliminate;
typedef boost::weak_ptr<DerivedType> derived_weak_ptr;
typedef typename ConditionalType::FactorType FactorType; protected:
typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
/// @name Standard Constructors
protected: /// @{
/// @name Standard Constructors /** Default constructor */
/// @{ BayesTreeCliqueBase() {}
/** Default constructor */ /** Construct from a conditional, leaving parent and child pointers uninitialized */
BayesTreeCliqueBase() {} BayesTreeCliqueBase(const sharedConditional& conditional) : conditional_(conditional) {}
/** Construct from a conditional, leaving parent and child pointers uninitialized */ /// @}
BayesTreeCliqueBase(const sharedConditional& conditional);
/// This stores the Cached separator margnal P(S)
/** Construct from an elimination result, which is a pair<CONDITIONAL,FACTOR> */ mutable boost::optional<FactorGraphType> cachedSeparatorMarginal_;
BayesTreeCliqueBase(
const std::pair<sharedConditional, public:
boost::shared_ptr<typename ConditionalType::FactorType> >& result); sharedConditional conditional_;
derived_weak_ptr parent_;
/// @} std::vector<derived_ptr> children;
/// This stores the Cached separator margnal P(S) /// @name Testable
mutable boost::optional<FactorGraph<FactorType> > cachedSeparatorMarginal_; /// @{
public: /** check equality */
sharedConditional conditional_; bool equals(const DERIVED& other, double tol = 1e-9) const;
derived_weak_ptr parent_;
FastList<derived_ptr> children_; /** print this node */
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @name Testable
/// @{ /// @}
/// @name Standard Interface
/** check equality */ /// @{
bool equals(const This& other, double tol = 1e-9) const {
return (!conditional_ && !other.conditional()) /** Access the conditional */
|| conditional_->equals(*other.conditional(), tol); const sharedConditional& conditional() const { return conditional_; }
}
/** is this the root of a Bayes tree ? */
/** print this node */ inline bool isRoot() const { return parent_.expired(); }
void print(const std::string& s = "", const IndexFormatter& indexFormatter =
DefaultIndexFormatter) const; /** The size of subtree rooted at this clique, i.e., nr of Cliques */
size_t treeSize() const;
/** print this node and entire subtree below it */
void printTree(const std::string& indent = "", /** Collect number of cliques with cached separator marginals */
const IndexFormatter& indexFormatter = DefaultIndexFormatter) const; size_t numCachedSeparatorMarginals() const;
/// @} /** return a shared_ptr to the parent clique */
/// @name Standard Interface derived_ptr parent() const { return parent_.lock(); }
/// @{
/// @}
/** Access the conditional */ /// @name Advanced Interface
const sharedConditional& conditional() const { /// @{
return conditional_;
} /** return the conditional P(S|Root) on the separator given the root */
BayesNetType shortcut(const derived_ptr& root, Eliminate function = EliminationTraits::DefaultEliminate) const;
/** is this the root of a Bayes tree ? */
inline bool isRoot() const { /** return the marginal P(S) on the separator */
return parent_.expired(); FactorGraphType separatorMarginal(Eliminate function = EliminationTraits::DefaultEliminate) const;
}
/** return the marginal P(C) of the clique, using marginal caching */
/** The size of subtree rooted at this clique, i.e., nr of Cliques */ FactorGraphType marginal2(Eliminate function = EliminationTraits::DefaultEliminate) const;
size_t treeSize() const;
/**
/** Collect number of cliques with cached separator marginals */ * This deletes the cached shortcuts of all cliques (subtree) below this clique.
size_t numCachedSeparatorMarginals() const; * This is performed when the bayes tree is modified.
*/
/** The arrow operator accesses the conditional */ void deleteCachedShortcuts();
const ConditionalType* operator->() const {
return conditional_.get(); const boost::optional<FactorGraphType>& cachedSeparatorMarginal() const {
} return cachedSeparatorMarginal_; }
/** return the const reference of children */ friend class BayesTree<DerivedType>;
const std::list<derived_ptr>& children() const {
return children_; protected:
}
/// Calculate set \f$ S \setminus B \f$ for shortcut calculations
/** return a shared_ptr to the parent clique */ std::vector<Key> separator_setminus_B(const derived_ptr& B) const;
derived_ptr parent() const {
return parent_.lock(); /** Determine variable indices to keep in recursive separator shortcut calculation The factor
} * graph p_Cp_B has keys from the parent clique Cp and from B. But we only keep the variables
* not in S union B. */
/// @} std::vector<Key> shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const;
/// @name Advanced Interface
/// @{ /** Non-recursive delete cached shortcuts and marginals - internal only. */
void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }
/** The arrow operator accesses the conditional */
ConditionalType* operator->() { private:
return conditional_.get();
} /** Serialization function */
friend class boost::serialization::access;
/** return the reference of children non-const version*/ template<class ARCHIVE>
FastList<derived_ptr>& children() { void serialize(ARCHIVE & ar, const unsigned int version) {
return children_; ar & BOOST_SERIALIZATION_NVP(conditional_);
} ar & BOOST_SERIALIZATION_NVP(parent_);
ar & BOOST_SERIALIZATION_NVP(children_);
/** Construct shared_ptr from a conditional, leaving parent and child pointers uninitialized */ }
static derived_ptr Create(const sharedConditional& conditional) {
return boost::make_shared<DerivedType>(conditional); /// @}
}
};
/** Construct shared_ptr from a FactorGraph<FACTOR>::EliminationResult. In this class
* the conditional part is kept and the factor part is ignored, but in derived clique }
* types, such as ISAM2Clique, the factor part is kept as a cached factor.
* @param result An elimination result, which is a pair<CONDITIONAL,FACTOR>
*/
static derived_ptr Create(const std::pair<sharedConditional,
boost::shared_ptr<typename ConditionalType::FactorType> >& result) {
return boost::make_shared<DerivedType>(result);
}
/** Returns a new clique containing a copy of the conditional but without
* the parent and child clique pointers.
*/
derived_ptr clone() const {
return Create(sharedConditional(new ConditionalType(*conditional_)));
}
/** Permute the variables in the whole subtree rooted at this clique */
void permuteWithInverse(const Permutation& inversePermutation);
/** Permute variables when they only appear in the separators. In this
* case the running intersection property will be used to prevent always
* traversing the whole tree. Returns whether any separator variables in
* this subtree were reordered.
*/
bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction);
/** return the conditional P(S|Root) on the separator given the root */
BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
/** return the marginal P(S) on the separator */
FactorGraph<FactorType> separatorMarginal(derived_ptr root, Eliminate function) const;
/** return the marginal P(C) of the clique, using marginal caching */
FactorGraph<FactorType> marginal2(derived_ptr root, Eliminate function) const;
/**
* This deletes the cached shortcuts of all cliques (subtree) below this clique.
* This is performed when the bayes tree is modified.
*/
void deleteCachedShortcuts();
const boost::optional<FactorGraph<FactorType> >& cachedSeparatorMarginal() const {
return cachedSeparatorMarginal_; }
friend class BayesTree<ConditionalType, DerivedType> ;
protected:
/// assert invariants that have to hold in a clique
void assertInvariants() const;
/// Calculate set \f$ S \setminus B \f$ for shortcut calculations
std::vector<Index> separator_setminus_B(derived_ptr B) const;
/// Calculate set \f$ S_p \cap B \f$ for shortcut calculations
std::vector<Index> parent_separator_intersection_B(derived_ptr B) const;
/**
* Determine variable indices to keep in recursive separator shortcut calculation
* The factor graph p_Cp_B has keys from the parent clique Cp and from B.
* But we only keep the variables not in S union B.
*/
std::vector<Index> shortcut_indices(derived_ptr B,
const FactorGraph<FactorType>& p_Cp_B) const;
/** Non-recursive delete cached shortcuts and marginals - internal only. */
void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }
private:
/**
* Cliques cannot be copied except by the clone() method, which does not
* copy the parent and child pointers.
*/
BayesTreeCliqueBase(const This& other) {
assert(false);
}
/** Cliques cannot be copied except by the clone() method, which does not
* copy the parent and child pointers.
*/
This& operator=(const This& other) {
assert(false);
return *this;
}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(conditional_);
ar & BOOST_SERIALIZATION_NVP(parent_);
ar & BOOST_SERIALIZATION_NVP(children_);
}
/// @}
};
// \struct Clique
template<class DERIVED, class CONDITIONAL>
const DERIVED* asDerived(
const BayesTreeCliqueBase<DERIVED, CONDITIONAL>* base) {
return static_cast<const DERIVED*>(base);
}
template<class DERIVED, class CONDITIONAL>
DERIVED* asDerived(BayesTreeCliqueBase<DERIVED, CONDITIONAL>* base) {
return static_cast<DERIVED*>(base);
}
}

44
gtsam/inference/BayesTreeCliqueBase-inl.h → gtsam/inference/BayesTreeCliqueBaseOrdered-inl.h
View File

@ -23,12 +23,12 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBase<DERIVED, CONDITIONAL>::assertInvariants() const { void BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::assertInvariants() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
std::vector<Index> BayesTreeCliqueBase<DERIVED, CONDITIONAL>::separator_setminus_B( std::vector<Index> BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::separator_setminus_B(
derived_ptr B) const { derived_ptr B) const {
sharedConditional p_F_S = this->conditional(); sharedConditional p_F_S = this->conditional();
std::vector<Index> &indicesB = B->conditional()->keys(); std::vector<Index> &indicesB = B->conditional()->keys();
@ -40,8 +40,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
std::vector<Index> BayesTreeCliqueBase<DERIVED, CONDITIONAL>::shortcut_indices( std::vector<Index> BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::shortcut_indices(
derived_ptr B, const FactorGraph<FactorType>& p_Cp_B) const derived_ptr B, const FactorGraphOrdered<FactorType>& p_Cp_B) const
{ {
gttic(shortcut_indices); gttic(shortcut_indices);
std::set<Index> allKeys = p_Cp_B.keys(); std::set<Index> allKeys = p_Cp_B.keys();
@ -60,7 +60,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
BayesTreeCliqueBase<DERIVED, CONDITIONAL>::BayesTreeCliqueBase( BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::BayesTreeCliqueBaseOrdered(
const sharedConditional& conditional) : const sharedConditional& conditional) :
conditional_(conditional) { conditional_(conditional) {
assertInvariants(); assertInvariants();
@ -68,7 +68,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
BayesTreeCliqueBase<DERIVED, CONDITIONAL>::BayesTreeCliqueBase( BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::BayesTreeCliqueBaseOrdered(
const std::pair<sharedConditional, const std::pair<sharedConditional,
boost::shared_ptr<typename ConditionalType::FactorType> >& result) : boost::shared_ptr<typename ConditionalType::FactorType> >& result) :
conditional_(result.first) { conditional_(result.first) {
@ -77,14 +77,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBase<DERIVED, CONDITIONAL>::print(const std::string& s, void BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::print(const std::string& s,
const IndexFormatter& indexFormatter) const { const IndexFormatter& indexFormatter) const {
conditional_->print(s, indexFormatter); conditional_->print(s, indexFormatter);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
size_t BayesTreeCliqueBase<DERIVED, CONDITIONAL>::treeSize() const { size_t BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::treeSize() const {
size_t size = 1; size_t size = 1;
BOOST_FOREACH(const derived_ptr& child, children_) BOOST_FOREACH(const derived_ptr& child, children_)
size += child->treeSize(); size += child->treeSize();
@ -93,7 +93,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
size_t BayesTreeCliqueBase<DERIVED, CONDITIONAL>::numCachedSeparatorMarginals() const { size_t BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::numCachedSeparatorMarginals() const {
if (!cachedSeparatorMarginal_) if (!cachedSeparatorMarginal_)
return 0; return 0;
@ -106,7 +106,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBase<DERIVED, CONDITIONAL>::printTree( void BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::printTree(
const std::string& indent, const IndexFormatter& indexFormatter) const { const std::string& indent, const IndexFormatter& indexFormatter) const {
asDerived(this)->print(indent, indexFormatter); asDerived(this)->print(indent, indexFormatter);
BOOST_FOREACH(const derived_ptr& child, children_) BOOST_FOREACH(const derived_ptr& child, children_)
@ -115,7 +115,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBase<DERIVED, CONDITIONAL>::permuteWithInverse( void BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::permuteWithInverse(
const Permutation& inversePermutation) { const Permutation& inversePermutation) {
conditional_->permuteWithInverse(inversePermutation); conditional_->permuteWithInverse(inversePermutation);
BOOST_FOREACH(const derived_ptr& child, children_) { BOOST_FOREACH(const derived_ptr& child, children_) {
@ -126,7 +126,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
bool BayesTreeCliqueBase<DERIVED, CONDITIONAL>::reduceSeparatorWithInverse( bool BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::reduceSeparatorWithInverse(
const internal::Reduction& inverseReduction) const internal::Reduction& inverseReduction)
{ {
bool changed = conditional_->reduceSeparatorWithInverse(inverseReduction); bool changed = conditional_->reduceSeparatorWithInverse(inverseReduction);
@ -150,7 +150,7 @@ namespace gtsam {
// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp 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
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
BayesNet<CONDITIONAL> BayesTreeCliqueBase<DERIVED, CONDITIONAL>::shortcut( BayesNetOrdered<CONDITIONAL> BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::shortcut(
derived_ptr B, Eliminate function) const derived_ptr B, Eliminate function) const
{ {
gttic(BayesTreeCliqueBase_shortcut); gttic(BayesTreeCliqueBase_shortcut);
@ -163,7 +163,7 @@ namespace gtsam {
// Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
derived_ptr parent(parent_.lock()); derived_ptr parent(parent_.lock());
gttoc(BayesTreeCliqueBase_shortcut); gttoc(BayesTreeCliqueBase_shortcut);
FactorGraph<FactorType> p_Cp_B(parent->shortcut(B, function)); // P(Sp||B) FactorGraphOrdered<FactorType> p_Cp_B(parent->shortcut(B, function)); // P(Sp||B)
gttic(BayesTreeCliqueBase_shortcut); gttic(BayesTreeCliqueBase_shortcut);
p_Cp_B.push_back(parent->conditional()->toFactor()); // P(Fp|Sp) p_Cp_B.push_back(parent->conditional()->toFactor()); // P(Fp|Sp)
@ -184,7 +184,7 @@ namespace gtsam {
// Finally, we only want to have S\B variables in the Bayes net, so // Finally, we only want to have S\B variables in the Bayes net, so
size_t nrFrontals = S_setminus_B.size(); size_t nrFrontals = S_setminus_B.size();
BayesNet<CONDITIONAL> result = *solver.conditionalBayesNet(keep, nrFrontals, function); BayesNetOrdered<CONDITIONAL> result = *solver.conditionalBayesNet(keep, nrFrontals, function);
// Undo the reduction // Undo the reduction
gttic(Undo_Reduce); gttic(Undo_Reduce);
@ -197,7 +197,7 @@ namespace gtsam {
return result; return result;
} else { } else {
return BayesNet<CONDITIONAL>(); return BayesNetOrdered<CONDITIONAL>();
} }
} }
@ -206,7 +206,7 @@ namespace gtsam {
// P(C) = P(F|S) P(S) // P(C) = P(F|S) P(S)
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
FactorGraph<typename BayesTreeCliqueBase<DERIVED, CONDITIONAL>::FactorType> BayesTreeCliqueBase< FactorGraphOrdered<typename BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::FactorType> BayesTreeCliqueBaseOrdered<
DERIVED, CONDITIONAL>::separatorMarginal(derived_ptr R, Eliminate function) const DERIVED, CONDITIONAL>::separatorMarginal(derived_ptr R, Eliminate function) const
{ {
gttic(BayesTreeCliqueBase_separatorMarginal); gttic(BayesTreeCliqueBase_separatorMarginal);
@ -216,7 +216,7 @@ namespace gtsam {
// If this is the root, there is no separator // If this is the root, there is no separator
if (R.get() == this) { if (R.get() == this) {
// we are root, return empty // we are root, return empty
FactorGraph<FactorType> empty; FactorGraphOrdered<FactorType> empty;
cachedSeparatorMarginal_ = empty; cachedSeparatorMarginal_ = empty;
} else { } else {
// Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp) // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
@ -224,7 +224,7 @@ namespace gtsam {
derived_ptr parent(parent_.lock()); derived_ptr parent(parent_.lock());
gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss); // Flatten recursion in timing outline gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss); // Flatten recursion in timing outline
gttoc(BayesTreeCliqueBase_separatorMarginal); gttoc(BayesTreeCliqueBase_separatorMarginal);
FactorGraph<FactorType> p_Cp(parent->separatorMarginal(R, function)); // P(Sp) FactorGraphOrdered<FactorType> p_Cp(parent->separatorMarginal(R, function)); // P(Sp)
gttic(BayesTreeCliqueBase_separatorMarginal); gttic(BayesTreeCliqueBase_separatorMarginal);
gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
// now add the parent conditional // now add the parent conditional
@ -269,12 +269,12 @@ namespace gtsam {
// P(C) = P(F|S) P(S) // P(C) = P(F|S) P(S)
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
FactorGraph<typename BayesTreeCliqueBase<DERIVED, CONDITIONAL>::FactorType> BayesTreeCliqueBase< FactorGraphOrdered<typename BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::FactorType> BayesTreeCliqueBaseOrdered<
DERIVED, CONDITIONAL>::marginal2(derived_ptr R, Eliminate function) const DERIVED, CONDITIONAL>::marginal2(derived_ptr R, Eliminate function) const
{ {
gttic(BayesTreeCliqueBase_marginal2); gttic(BayesTreeCliqueBase_marginal2);
// initialize with separator marginal P(S) // initialize with separator marginal P(S)
FactorGraph<FactorType> p_C(this->separatorMarginal(R, function)); FactorGraphOrdered<FactorType> p_C(this->separatorMarginal(R, function));
// add the conditional P(F|S) // add the conditional P(F|S)
p_C.push_back(this->conditional()->toFactor()); p_C.push_back(this->conditional()->toFactor());
return p_C; return p_C;
@ -282,7 +282,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBase<DERIVED, CONDITIONAL>::deleteCachedShortcuts() { void BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::deleteCachedShortcuts() {
// When a shortcut is requested, all of the shortcuts between it and the // When a shortcut is requested, all of the shortcuts between it and the
// root are also generated. So, if this clique's cached shortcut is set, // root are also generated. So, if this clique's cached shortcut is set,

273
gtsam/inference/BayesTreeCliqueBaseOrdered.h Normal file
View File

@ -0,0 +1,273 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file BayesTreeCliqueBase.h
* @brief Base class for cliques of a BayesTree
* @author Richard Roberts and Frank Dellaert
*/
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/weak_ptr.hpp>
#include <gtsam/global_includes.h>
#include <gtsam/inference/FactorGraphOrdered.h>
#include <gtsam/inference/BayesNetOrdered.h>
namespace gtsam {
template<class CONDITIONAL, class CLIQUE> class BayesTreeOrdered;
}
namespace gtsam {
/**
* This is the base class for BayesTree cliques. The default and standard
* derived type is BayesTreeClique, but some algorithms, like iSAM2, use a
* different clique type in order to store extra data along with the clique.
*
* This class is templated on the derived class (i.e. the curiously recursive
* template pattern). The advantage of this over using virtual classes is
* that it avoids the need for casting to get the derived type. This is
* possible because all cliques in a BayesTree are the same type - if they
* were not then we'd need a virtual class.
*
* @tparam DERIVED The derived clique type.
* @tparam CONDITIONAL The conditional type.
* \nosubgrouping
*/
template<class DERIVED, class CONDITIONAL>
struct BayesTreeCliqueBaseOrdered {
public:
typedef BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL> This;
typedef DERIVED DerivedType;
typedef CONDITIONAL ConditionalType;
typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
typedef boost::shared_ptr<DerivedType> derived_ptr;
typedef boost::weak_ptr<DerivedType> derived_weak_ptr;
typedef typename ConditionalType::FactorType FactorType;
typedef typename FactorGraphOrdered<FactorType>::Eliminate Eliminate;
protected:
/// @name Standard Constructors
/// @{
/** Default constructor */
BayesTreeCliqueBaseOrdered() {}
/** Construct from a conditional, leaving parent and child pointers uninitialized */
BayesTreeCliqueBaseOrdered(const sharedConditional& conditional);
/** Construct from an elimination result, which is a pair<CONDITIONAL,FACTOR> */
BayesTreeCliqueBaseOrdered(
const std::pair<sharedConditional,
boost::shared_ptr<typename ConditionalType::FactorType> >& result);
/// @}
/// This stores the Cached separator margnal P(S)
mutable boost::optional<FactorGraphOrdered<FactorType> > cachedSeparatorMarginal_;
public:
sharedConditional conditional_;
derived_weak_ptr parent_;
FastList<derived_ptr> children_;
/// @name Testable
/// @{
/** check equality */
bool equals(const This& other, double tol = 1e-9) const {
return (!conditional_ && !other.conditional())
|| conditional_->equals(*other.conditional(), tol);
}
/** print this node */
void print(const std::string& s = "", const IndexFormatter& indexFormatter =
DefaultIndexFormatter) const;
/** print this node and entire subtree below it */
void printTree(const std::string& indent = "",
const IndexFormatter& indexFormatter = DefaultIndexFormatter) const;
/// @}
/// @name Standard Interface
/// @{
/** Access the conditional */
const sharedConditional& conditional() const {
return conditional_;
}
/** is this the root of a Bayes tree ? */
inline bool isRoot() const {
return parent_.expired();
}
/** The size of subtree rooted at this clique, i.e., nr of Cliques */
size_t treeSize() const;
/** Collect number of cliques with cached separator marginals */
size_t numCachedSeparatorMarginals() const;
/** The arrow operator accesses the conditional */
const ConditionalType* operator->() const {
return conditional_.get();
}
/** return the const reference of children */
const std::list<derived_ptr>& children() const {
return children_;
}
/** return a shared_ptr to the parent clique */
derived_ptr parent() const {
return parent_.lock();
}
/// @}
/// @name Advanced Interface
/// @{
/** The arrow operator accesses the conditional */
ConditionalType* operator->() {
return conditional_.get();
}
/** return the reference of children non-const version*/
FastList<derived_ptr>& children() {
return children_;
}
/** Construct shared_ptr from a conditional, leaving parent and child pointers uninitialized */
static derived_ptr Create(const sharedConditional& conditional) {
return boost::make_shared<DerivedType>(conditional);
}
/** Construct shared_ptr from a FactorGraph<FACTOR>::EliminationResult. In this class
* the conditional part is kept and the factor part is ignored, but in derived clique
* types, such as ISAM2Clique, the factor part is kept as a cached factor.
* @param result An elimination result, which is a pair<CONDITIONAL,FACTOR>
*/
static derived_ptr Create(const std::pair<sharedConditional,
boost::shared_ptr<typename ConditionalType::FactorType> >& result) {
return boost::make_shared<DerivedType>(result);
}
/** Returns a new clique containing a copy of the conditional but without
* the parent and child clique pointers.
*/
derived_ptr clone() const {
return Create(sharedConditional(new ConditionalType(*conditional_)));
}
/** Permute the variables in the whole subtree rooted at this clique */
void permuteWithInverse(const Permutation& inversePermutation);
/** Permute variables when they only appear in the separators. In this
* case the running intersection property will be used to prevent always
* traversing the whole tree. Returns whether any separator variables in
* this subtree were reordered.
*/
bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction);
/** return the conditional P(S|Root) on the separator given the root */
BayesNetOrdered<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
/** return the marginal P(S) on the separator */
FactorGraphOrdered<FactorType> separatorMarginal(derived_ptr root, Eliminate function) const;
/** return the marginal P(C) of the clique, using marginal caching */
FactorGraphOrdered<FactorType> marginal2(derived_ptr root, Eliminate function) const;
/**
* This deletes the cached shortcuts of all cliques (subtree) below this clique.
* This is performed when the bayes tree is modified.
*/
void deleteCachedShortcuts();
const boost::optional<FactorGraphOrdered<FactorType> >& cachedSeparatorMarginal() const {
return cachedSeparatorMarginal_; }
friend class BayesTreeOrdered<ConditionalType, DerivedType> ;
protected:
/// assert invariants that have to hold in a clique
void assertInvariants() const;
/// Calculate set \f$ S \setminus B \f$ for shortcut calculations
std::vector<Index> separator_setminus_B(derived_ptr B) const;
/// Calculate set \f$ S_p \cap B \f$ for shortcut calculations
std::vector<Index> parent_separator_intersection_B(derived_ptr B) const;
/**
* Determine variable indices to keep in recursive separator shortcut calculation
* The factor graph p_Cp_B has keys from the parent clique Cp and from B.
* But we only keep the variables not in S union B.
*/
std::vector<Index> shortcut_indices(derived_ptr B,
const FactorGraphOrdered<FactorType>& p_Cp_B) const;
/** Non-recursive delete cached shortcuts and marginals - internal only. */
void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }
private:
/**
* Cliques cannot be copied except by the clone() method, which does not
* copy the parent and child pointers.
*/
BayesTreeCliqueBaseOrdered(const This& other) {
assert(false);
}
/** Cliques cannot be copied except by the clone() method, which does not
* copy the parent and child pointers.
*/
This& operator=(const This& other) {
assert(false);
return *this;
}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(conditional_);
ar & BOOST_SERIALIZATION_NVP(parent_);
ar & BOOST_SERIALIZATION_NVP(children_);
}
/// @}
};
// \struct Clique
template<class DERIVED, class CONDITIONAL>
const DERIVED* asDerived(
const BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>* base) {
return static_cast<const DERIVED*>(base);
}
template<class DERIVED, class CONDITIONAL>
DERIVED* asDerived(BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>* base) {
return static_cast<DERIVED*>(base);
}
}

162
gtsam/inference/BayesTreeCliqueBaseUnordered.h
View File

@ -1,162 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file BayesTreeCliqueBase.h
* @brief Base class for cliques of a BayesTree
* @author Richard Roberts and Frank Dellaert
*/
#pragma once
#include <gtsam/base/types.h>
namespace gtsam {
// Forward declarations
template<class CLIQUE> class BayesTreeUnordered;
template<class GRAPH> struct EliminationTraits;
/**
* This is the base class for BayesTree cliques. The default and standard derived type is
* BayesTreeClique, but some algorithms, like iSAM2, use a different clique type in order to store
* extra data along with the clique.
*
* This class is templated on the derived class (i.e. the curiously recursive template pattern).
* The advantage of this over using virtual classes is that it avoids the need for casting to get
* the derived type. This is possible because all cliques in a BayesTree are the same type - if
* they were not then we'd need a virtual class.
*
* @tparam DERIVED The derived clique type.
* @tparam CONDITIONAL The conditional type.
* \nosubgrouping */
template<class DERIVED, class FACTORGRAPH>
class BayesTreeCliqueBaseUnordered
{
private:
typedef BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH> This;
typedef DERIVED DerivedType;
typedef EliminationTraits<FACTORGRAPH> EliminationTraits;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
typedef boost::shared_ptr<DerivedType> derived_ptr;
typedef boost::weak_ptr<DerivedType> derived_weak_ptr;
public:
typedef FACTORGRAPH FactorGraphType;
typedef typename EliminationTraits::BayesNetType BayesNetType;
typedef typename BayesNetType::ConditionalType ConditionalType;
typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef typename FactorGraphType::FactorType FactorType;
typedef typename FactorGraphType::Eliminate Eliminate;
protected:
/// @name Standard Constructors
/// @{
/** Default constructor */
BayesTreeCliqueBaseUnordered() {}
/** Construct from a conditional, leaving parent and child pointers uninitialized */
BayesTreeCliqueBaseUnordered(const sharedConditional& conditional) : conditional_(conditional) {}
/// @}
/// This stores the Cached separator margnal P(S)
mutable boost::optional<FactorGraphType> cachedSeparatorMarginal_;
public:
sharedConditional conditional_;
derived_weak_ptr parent_;
std::vector<derived_ptr> children;
/// @name Testable
/// @{
/** check equality */
bool equals(const DERIVED& other, double tol = 1e-9) const;
/** print this node */
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @}
/// @name Standard Interface
/// @{
/** Access the conditional */
const sharedConditional& conditional() const { return conditional_; }
/** is this the root of a Bayes tree ? */
inline bool isRoot() const { return parent_.expired(); }
/** The size of subtree rooted at this clique, i.e., nr of Cliques */
size_t treeSize() const;
/** Collect number of cliques with cached separator marginals */
size_t numCachedSeparatorMarginals() const;
/** return a shared_ptr to the parent clique */
derived_ptr parent() const { return parent_.lock(); }
/// @}
/// @name Advanced Interface
/// @{
/** return the conditional P(S|Root) on the separator given the root */
BayesNetType shortcut(const derived_ptr& root, Eliminate function = EliminationTraits::DefaultEliminate) const;
/** return the marginal P(S) on the separator */
FactorGraphType separatorMarginal(Eliminate function = EliminationTraits::DefaultEliminate) const;
/** return the marginal P(C) of the clique, using marginal caching */
FactorGraphType marginal2(Eliminate function = EliminationTraits::DefaultEliminate) const;
/**
* This deletes the cached shortcuts of all cliques (subtree) below this clique.
* This is performed when the bayes tree is modified.
*/
void deleteCachedShortcuts();
const boost::optional<FactorGraphType>& cachedSeparatorMarginal() const {
return cachedSeparatorMarginal_; }
friend class BayesTreeUnordered<DerivedType>;
protected:
/// Calculate set \f$ S \setminus B \f$ for shortcut calculations
std::vector<Key> separator_setminus_B(const derived_ptr& B) const;
/** Determine variable indices to keep in recursive separator shortcut calculation The factor
* graph p_Cp_B has keys from the parent clique Cp and from B. But we only keep the variables
* not in S union B. */
std::vector<Key> shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const;
/** Non-recursive delete cached shortcuts and marginals - internal only. */
void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(conditional_);
ar & BOOST_SERIALIZATION_NVP(parent_);
ar & BOOST_SERIALIZATION_NVP(children_);
}
/// @}
};
}

140
gtsam/inference/BayesTree-inl.h → gtsam/inference/BayesTreeOrdered-inl.h
View File

@ -23,8 +23,8 @@
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
#include <gtsam/base/FastSet.h> #include <gtsam/base/FastSet.h>
#include <gtsam/base/FastVector.h> #include <gtsam/base/FastVector.h>
#include <gtsam/inference/BayesTree.h> #include <gtsam/inference/BayesTreeOrdered.h>
#include <gtsam/inference/inference.h> #include <gtsam/inference/inferenceOrdered.h>
#include <gtsam/inference/GenericSequentialSolver.h> #include <gtsam/inference/GenericSequentialSolver.h>
#include <fstream> #include <fstream>
@ -40,8 +40,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::CliqueData typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::CliqueData
BayesTree<CONDITIONAL,CLIQUE>::getCliqueData() const { BayesTreeOrdered<CONDITIONAL,CLIQUE>::getCliqueData() const {
CliqueData data; CliqueData data;
getCliqueData(data, root_); getCliqueData(data, root_);
return data; return data;
@ -49,7 +49,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::getCliqueData(CliqueData& data, sharedClique clique) const { void BayesTreeOrdered<CONDITIONAL,CLIQUE>::getCliqueData(CliqueData& data, sharedClique clique) const {
data.conditionalSizes.push_back((*clique)->nrFrontals()); data.conditionalSizes.push_back((*clique)->nrFrontals());
data.separatorSizes.push_back((*clique)->nrParents()); data.separatorSizes.push_back((*clique)->nrParents());
BOOST_FOREACH(sharedClique c, clique->children_) { BOOST_FOREACH(sharedClique c, clique->children_) {
@ -59,13 +59,13 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
size_t BayesTree<CONDITIONAL,CLIQUE>::numCachedSeparatorMarginals() const { size_t BayesTreeOrdered<CONDITIONAL,CLIQUE>::numCachedSeparatorMarginals() const {
return (root_) ? root_->numCachedSeparatorMarginals() : 0; return (root_) ? root_->numCachedSeparatorMarginals() : 0;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(const std::string &s, const IndexFormatter& indexFormatter) const { void BayesTreeOrdered<CONDITIONAL,CLIQUE>::saveGraph(const std::string &s, const IndexFormatter& indexFormatter) const {
if (!root_.get()) throw std::invalid_argument("the root of Bayes tree has not been initialized!"); if (!root_.get()) throw std::invalid_argument("the root of Bayes tree has not been initialized!");
std::ofstream of(s.c_str()); std::ofstream of(s.c_str());
of<< "digraph G{\n"; of<< "digraph G{\n";
@ -76,7 +76,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, int parentnum) const { void BayesTreeOrdered<CONDITIONAL,CLIQUE>::saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, int parentnum) const {
static int num = 0; static int num = 0;
bool first = true; bool first = true;
std::stringstream out; std::stringstream out;
@ -112,7 +112,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::CliqueStats::print(const std::string& s) const { void BayesTreeOrdered<CONDITIONAL,CLIQUE>::CliqueStats::print(const std::string& s) const {
std::cout << s std::cout << s
<<"avg Conditional Size: " << avgConditionalSize << std::endl <<"avg Conditional Size: " << avgConditionalSize << std::endl
<< "max Conditional Size: " << maxConditionalSize << std::endl << "max Conditional Size: " << maxConditionalSize << std::endl
@ -122,8 +122,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::CliqueStats typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::CliqueStats
BayesTree<CONDITIONAL,CLIQUE>::CliqueData::getStats() const { BayesTreeOrdered<CONDITIONAL,CLIQUE>::CliqueData::getStats() const {
CliqueStats stats; CliqueStats stats;
double sum = 0.0; double sum = 0.0;
@ -149,7 +149,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::Cliques::print(const std::string& s, const IndexFormatter& indexFormatter) const { void BayesTreeOrdered<CONDITIONAL,CLIQUE>::Cliques::print(const std::string& s, const IndexFormatter& indexFormatter) const {
std::cout << s << ":\n"; std::cout << s << ":\n";
BOOST_FOREACH(sharedClique clique, *this) BOOST_FOREACH(sharedClique clique, *this)
clique->printTree("", indexFormatter); clique->printTree("", indexFormatter);
@ -157,14 +157,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
bool BayesTree<CONDITIONAL,CLIQUE>::Cliques::equals(const Cliques& other, double tol) const { bool BayesTreeOrdered<CONDITIONAL,CLIQUE>::Cliques::equals(const Cliques& other, double tol) const {
return other == *this; return other == *this;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique
BayesTree<CONDITIONAL,CLIQUE>::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) { BayesTreeOrdered<CONDITIONAL,CLIQUE>::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) {
sharedClique new_clique(new Clique(conditional)); sharedClique new_clique(new Clique(conditional));
addClique(new_clique, parent_clique); addClique(new_clique, parent_clique);
return new_clique; return new_clique;
@ -172,7 +172,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) { void BayesTreeOrdered<CONDITIONAL,CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size())); nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size()));
BOOST_FOREACH(Index j, (*clique)->frontals()) BOOST_FOREACH(Index j, (*clique)->frontals())
nodes_[j] = clique; nodes_[j] = clique;
@ -188,7 +188,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique BayesTree<CONDITIONAL,CLIQUE>::addClique( typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique BayesTreeOrdered<CONDITIONAL,CLIQUE>::addClique(
const sharedConditional& conditional, std::list<sharedClique>& child_cliques) { const sharedConditional& conditional, std::list<sharedClique>& child_cliques) {
sharedClique new_clique(new Clique(conditional)); sharedClique new_clique(new Clique(conditional));
nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size())); nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size()));
@ -203,7 +203,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::permuteWithInverse(const Permutation& inversePermutation) { void BayesTreeOrdered<CONDITIONAL,CLIQUE>::permuteWithInverse(const Permutation& inversePermutation) {
// recursively permute the cliques and internal conditionals // recursively permute the cliques and internal conditionals
if (root_) if (root_)
root_->permuteWithInverse(inversePermutation); root_->permuteWithInverse(inversePermutation);
@ -212,7 +212,7 @@ namespace gtsam {
Index maxIndex = *std::max_element(inversePermutation.begin(), inversePermutation.end()); Index maxIndex = *std::max_element(inversePermutation.begin(), inversePermutation.end());
// Update the nodes structure // Update the nodes structure
typename BayesTree<CONDITIONAL,CLIQUE>::Nodes newNodes(maxIndex+1); typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::Nodes newNodes(maxIndex+1);
// inversePermutation.applyToCollection(newNodes, nodes_); // Uses the forward, rather than inverse permutation // inversePermutation.applyToCollection(newNodes, nodes_); // Uses the forward, rather than inverse permutation
for(size_t i = 0; i < nodes_.size(); ++i) for(size_t i = 0; i < nodes_.size(); ++i)
newNodes[inversePermutation[i]] = nodes_[i]; newNodes[inversePermutation[i]] = nodes_[i];
@ -222,7 +222,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
inline void BayesTree<CONDITIONAL,CLIQUE>::addToCliqueFront(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional, const sharedClique& clique) { inline void BayesTreeOrdered<CONDITIONAL,CLIQUE>::addToCliqueFront(BayesTreeOrdered<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional, const sharedClique& clique) {
static const bool debug = false; static const bool debug = false;
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
// Debug check to make sure the conditional variable is ordered lower than // Debug check to make sure the conditional variable is ordered lower than
@ -249,7 +249,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::removeClique(sharedClique clique) { void BayesTreeOrdered<CONDITIONAL,CLIQUE>::removeClique(sharedClique clique) {
if (clique->isRoot()) if (clique->isRoot())
root_.reset(); root_.reset();
@ -271,9 +271,9 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditional> >& symbolic, void BayesTreeOrdered<CONDITIONAL,CLIQUE>::recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditionalOrdered> >& symbolic,
const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals, const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals,
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& parent) { const typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique& parent) {
// Helper function to build a non-symbolic tree (e.g. Gaussian) using a // Helper function to build a non-symbolic tree (e.g. Gaussian) using a
// symbolic tree, used in the BT(BN) constructor. // symbolic tree, used in the BT(BN) constructor.
@ -282,21 +282,21 @@ namespace gtsam {
FastList<typename CONDITIONAL::shared_ptr> cliqueConditionals; FastList<typename CONDITIONAL::shared_ptr> cliqueConditionals;
BOOST_FOREACH(Index j, symbolic->conditional()->frontals()) { BOOST_FOREACH(Index j, symbolic->conditional()->frontals()) {
cliqueConditionals.push_back(conditionals[j]); } cliqueConditionals.push_back(conditionals[j]); }
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique thisClique(new CLIQUE(CONDITIONAL::Combine(cliqueConditionals.begin(), cliqueConditionals.end()))); typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique thisClique(new CLIQUE(CONDITIONAL::Combine(cliqueConditionals.begin(), cliqueConditionals.end())));
// Add the new clique with the current parent // Add the new clique with the current parent
this->addClique(thisClique, parent); this->addClique(thisClique, parent);
// Build the children, whose parent is the new clique // Build the children, whose parent is the new clique
BOOST_FOREACH(const BayesTree<IndexConditional>::sharedClique& child, symbolic->children()) { BOOST_FOREACH(const BayesTreeOrdered<IndexConditionalOrdered>::sharedClique& child, symbolic->children()) {
this->recursiveTreeBuild(child, conditionals, thisClique); } this->recursiveTreeBuild(child, conditionals, thisClique); }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet) { BayesTreeOrdered<CONDITIONAL,CLIQUE>::BayesTreeOrdered(const BayesNetOrdered<CONDITIONAL>& bayesNet) {
// First generate symbolic BT to determine clique structure // First generate symbolic BT to determine clique structure
BayesTree<IndexConditional> sbt(bayesNet); BayesTreeOrdered<IndexConditionalOrdered> sbt(bayesNet);
// Build index of variables to conditionals // Build index of variables to conditionals
std::vector<boost::shared_ptr<CONDITIONAL> > conditionals(sbt.root()->conditional()->frontals().back() + 1); std::vector<boost::shared_ptr<CONDITIONAL> > conditionals(sbt.root()->conditional()->frontals().back() + 1);
@ -317,21 +317,21 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<> template<>
inline BayesTree<IndexConditional>::BayesTree(const BayesNet<IndexConditional>& bayesNet) { inline BayesTreeOrdered<IndexConditionalOrdered>::BayesTreeOrdered(const BayesNetOrdered<IndexConditionalOrdered>& bayesNet) {
BayesNet<IndexConditional>::const_reverse_iterator rit; BayesNetOrdered<IndexConditionalOrdered>::const_reverse_iterator rit;
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
insert(*this, *rit); insert(*this, *rit);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL,CLIQUE> > subtrees) { BayesTreeOrdered<CONDITIONAL,CLIQUE>::BayesTreeOrdered(const BayesNetOrdered<CONDITIONAL>& bayesNet, std::list<BayesTreeOrdered<CONDITIONAL,CLIQUE> > subtrees) {
if (bayesNet.size() == 0) if (bayesNet.size() == 0)
throw std::invalid_argument("BayesTree::insert: empty bayes net!"); throw std::invalid_argument("BayesTree::insert: empty bayes net!");
// get the roots of child subtrees and merge their nodes_ // get the roots of child subtrees and merge their nodes_
std::list<sharedClique> childRoots; std::list<sharedClique> childRoots;
typedef BayesTree<CONDITIONAL,CLIQUE> Tree; typedef BayesTreeOrdered<CONDITIONAL,CLIQUE> Tree;
BOOST_FOREACH(const Tree& subtree, subtrees) { BOOST_FOREACH(const Tree& subtree, subtrees) {
nodes_.assign(subtree.nodes_.begin(), subtree.nodes_.end()); nodes_.assign(subtree.nodes_.begin(), subtree.nodes_.end());
childRoots.push_back(subtree.root()); childRoots.push_back(subtree.root());
@ -339,7 +339,7 @@ namespace gtsam {
// create a new clique and add all the conditionals to the clique // create a new clique and add all the conditionals to the clique
sharedClique new_clique; sharedClique new_clique;
typename BayesNet<CONDITIONAL>::sharedConditional conditional; typename BayesNetOrdered<CONDITIONAL>::sharedConditional conditional;
BOOST_REVERSE_FOREACH(conditional, bayesNet) { BOOST_REVERSE_FOREACH(conditional, bayesNet) {
if (!new_clique.get()) if (!new_clique.get())
new_clique = addClique(conditional,childRoots); new_clique = addClique(conditional,childRoots);
@ -352,13 +352,13 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const This& other) { BayesTreeOrdered<CONDITIONAL,CLIQUE>::BayesTreeOrdered(const This& other) {
*this = other; *this = other;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
BayesTree<CONDITIONAL,CLIQUE>& BayesTree<CONDITIONAL,CLIQUE>::operator=(const This& other) { BayesTreeOrdered<CONDITIONAL,CLIQUE>& BayesTreeOrdered<CONDITIONAL,CLIQUE>::operator=(const This& other) {
this->clear(); this->clear();
other.cloneTo(*this); other.cloneTo(*this);
return *this; return *this;
@ -366,7 +366,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::print(const std::string& s, const IndexFormatter& indexFormatter) const { void BayesTreeOrdered<CONDITIONAL,CLIQUE>::print(const std::string& s, const IndexFormatter& indexFormatter) const {
if (root_.use_count() == 0) { if (root_.use_count() == 0) {
std::cout << "WARNING: BayesTree.print encountered a forest..." << std::endl; std::cout << "WARNING: BayesTree.print encountered a forest..." << std::endl;
return; return;
@ -380,15 +380,15 @@ namespace gtsam {
// binary predicate to test equality of a pair for use in equals // binary predicate to test equality of a pair for use in equals
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
bool check_sharedCliques( bool check_sharedCliques(
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& v1, const typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique& v1,
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& v2 const typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique& v2
) { ) {
return (!v1 && !v2) || (v1 && v2 && v1->equals(*v2)); return (!v1 && !v2) || (v1 && v2 && v1->equals(*v2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
bool BayesTree<CONDITIONAL,CLIQUE>::equals(const BayesTree<CONDITIONAL,CLIQUE>& other, bool BayesTreeOrdered<CONDITIONAL,CLIQUE>::equals(const BayesTreeOrdered<CONDITIONAL,CLIQUE>& other,
double tol) const { double tol) const {
return size()==other.size() && return size()==other.size() &&
std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CONDITIONAL,CLIQUE>); std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CONDITIONAL,CLIQUE>);
@ -397,7 +397,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
template<class CONTAINER> template<class CONTAINER>
inline Index BayesTree<CONDITIONAL,CLIQUE>::findParentClique(const CONTAINER& parents) const { inline Index BayesTreeOrdered<CONDITIONAL,CLIQUE>::findParentClique(const CONTAINER& parents) const {
typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end()); typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end());
assert(lowestOrderedParent != parents.end()); assert(lowestOrderedParent != parents.end());
return *lowestOrderedParent; return *lowestOrderedParent;
@ -405,7 +405,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::insert(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional) void BayesTreeOrdered<CONDITIONAL,CLIQUE>::insert(BayesTreeOrdered<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional)
{ {
static const bool debug = false; static const bool debug = false;
@ -442,7 +442,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
//TODO: remove this function after removing TSAM.cpp //TODO: remove this function after removing TSAM.cpp
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique BayesTree<CONDITIONAL,CLIQUE>::insert( typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique BayesTreeOrdered<CONDITIONAL,CLIQUE>::insert(
const sharedConditional& clique, std::list<sharedClique>& children, bool isRootClique) { const sharedConditional& clique, std::list<sharedClique>& children, bool isRootClique) {
if (clique->nrFrontals() == 0) if (clique->nrFrontals() == 0)
@ -457,18 +457,18 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::fillNodesIndex(const sharedClique& subtree) { void BayesTreeOrdered<CONDITIONAL,CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
// Add each frontal variable of this root node // Add each frontal variable of this root node
BOOST_FOREACH(const Index& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; } BOOST_FOREACH(const Index& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; }
// Fill index for each child // Fill index for each child
typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique; typedef typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& child, subtree->children_) { BOOST_FOREACH(const sharedClique& child, subtree->children_) {
fillNodesIndex(child); } fillNodesIndex(child); }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::insert(const sharedClique& subtree) { void BayesTreeOrdered<CONDITIONAL,CLIQUE>::insert(const sharedClique& subtree) {
if(subtree) { if(subtree) {
// Find the parent clique of the new subtree. By the running intersection // Find the parent clique of the new subtree. By the running intersection
// property, those separator variables in the subtree that are ordered // property, those separator variables in the subtree that are ordered
@ -497,7 +497,7 @@ namespace gtsam {
// First finds clique marginal then marginalizes that // First finds clique marginal then marginalizes that
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
typename CONDITIONAL::FactorType::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::marginalFactor( typename CONDITIONAL::FactorType::shared_ptr BayesTreeOrdered<CONDITIONAL,CLIQUE>::marginalFactor(
Index j, Eliminate function) const Index j, Eliminate function) const
{ {
gttic(BayesTree_marginalFactor); gttic(BayesTree_marginalFactor);
@ -507,9 +507,9 @@ namespace gtsam {
// calculate or retrieve its marginal P(C) = P(F,S) // calculate or retrieve its marginal P(C) = P(F,S)
#ifdef OLD_SHORTCUT_MARGINALS #ifdef OLD_SHORTCUT_MARGINALS
FactorGraph<FactorType> cliqueMarginal = clique->marginal(root_,function); FactorGraphOrdered<FactorType> cliqueMarginal = clique->marginal(root_,function);
#else #else
FactorGraph<FactorType> cliqueMarginal = clique->marginal2(root_,function); FactorGraphOrdered<FactorType> cliqueMarginal = clique->marginal2(root_,function);
#endif #endif
// Reduce the variable indices to start at zero // Reduce the variable indices to start at zero
@ -535,13 +535,13 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::marginalBayesNet( typename BayesNetOrdered<CONDITIONAL>::shared_ptr BayesTreeOrdered<CONDITIONAL,CLIQUE>::marginalBayesNet(
Index j, Eliminate function) const Index j, Eliminate function) const
{ {
gttic(BayesTree_marginalBayesNet); gttic(BayesTree_marginalBayesNet);
// calculate marginal as a factor graph // calculate marginal as a factor graph
FactorGraph<FactorType> fg; FactorGraphOrdered<FactorType> fg;
fg.push_back(this->marginalFactor(j,function)); fg.push_back(this->marginalFactor(j,function));
// Reduce the variable indices to start at zero // Reduce the variable indices to start at zero
@ -553,7 +553,7 @@ namespace gtsam {
gttoc(Reduce); gttoc(Reduce);
// eliminate factor graph marginal to a Bayes net // eliminate factor graph marginal to a Bayes net
boost::shared_ptr<BayesNet<CONDITIONAL> > bn = GenericSequentialSolver<FactorType>(fg).eliminate(function); boost::shared_ptr<BayesNetOrdered<CONDITIONAL> > bn = GenericSequentialSolver<FactorType>(fg).eliminate(function);
// Undo the reduction // Undo the reduction
gttic(Undo_Reduce); gttic(Undo_Reduce);
@ -568,8 +568,8 @@ namespace gtsam {
// Find two cliques, their joint, then marginalizes // Find two cliques, their joint, then marginalizes
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
typename FactorGraph<typename CONDITIONAL::FactorType>::shared_ptr typename FactorGraphOrdered<typename CONDITIONAL::FactorType>::shared_ptr
BayesTree<CONDITIONAL,CLIQUE>::joint(Index j1, Index j2, Eliminate function) const { BayesTreeOrdered<CONDITIONAL,CLIQUE>::joint(Index j1, Index j2, Eliminate function) const {
gttic(BayesTree_joint); gttic(BayesTree_joint);
// get clique C1 and C2 // get clique C1 and C2
@ -605,13 +605,13 @@ namespace gtsam {
// Compute marginal on lowest common ancestor clique // Compute marginal on lowest common ancestor clique
gttic(LCA_marginal); gttic(LCA_marginal);
FactorGraph<FactorType> p_B = B->marginal2(this->root(), function); FactorGraphOrdered<FactorType> p_B = B->marginal2(this->root(), function);
gttoc(LCA_marginal); gttoc(LCA_marginal);
// Compute shortcuts of the requested cliques given the lowest common ancestor // Compute shortcuts of the requested cliques given the lowest common ancestor
gttic(Clique_shortcuts); gttic(Clique_shortcuts);
BayesNet<CONDITIONAL> p_C1_Bred = C1->shortcut(B, function); BayesNetOrdered<CONDITIONAL> p_C1_Bred = C1->shortcut(B, function);
BayesNet<CONDITIONAL> p_C2_Bred = C2->shortcut(B, function); BayesNetOrdered<CONDITIONAL> p_C2_Bred = C2->shortcut(B, function);
gttoc(Clique_shortcuts); gttoc(Clique_shortcuts);
// Factor the shortcuts to be conditioned on the full root // Factor the shortcuts to be conditioned on the full root
@ -625,8 +625,8 @@ namespace gtsam {
C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end()); C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end());
} }
// Factor into C1\B | B. // Factor into C1\B | B.
FactorGraph<FactorType> temp_remaining; FactorGraphOrdered<FactorType> temp_remaining;
boost::tie(p_C1_B, temp_remaining) = FactorGraph<FactorType>(p_C1_Bred).eliminate(C1_minus_B, function); boost::tie(p_C1_B, temp_remaining) = FactorGraphOrdered<FactorType>(p_C1_Bred).eliminate(C1_minus_B, function);
} }
sharedConditional p_C2_B; { sharedConditional p_C2_B; {
std::vector<Index> C2_minus_B; { std::vector<Index> C2_minus_B; {
@ -636,14 +636,14 @@ namespace gtsam {
C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end()); C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end());
} }
// Factor into C2\B | B. // Factor into C2\B | B.
FactorGraph<FactorType> temp_remaining; FactorGraphOrdered<FactorType> temp_remaining;
boost::tie(p_C2_B, temp_remaining) = FactorGraph<FactorType>(p_C2_Bred).eliminate(C2_minus_B, function); boost::tie(p_C2_B, temp_remaining) = FactorGraphOrdered<FactorType>(p_C2_Bred).eliminate(C2_minus_B, function);
} }
gttoc(Full_root_factoring); gttoc(Full_root_factoring);
gttic(Variable_joint); gttic(Variable_joint);
// Build joint on all involved variables // Build joint on all involved variables
FactorGraph<FactorType> p_BC1C2; FactorGraphOrdered<FactorType> p_BC1C2;
p_BC1C2.push_back(p_B); p_BC1C2.push_back(p_B);
p_BC1C2.push_back(p_C1_B->toFactor()); p_BC1C2.push_back(p_C1_B->toFactor());
p_BC1C2.push_back(p_C2_B->toFactor()); p_BC1C2.push_back(p_C2_B->toFactor());
@ -663,7 +663,7 @@ namespace gtsam {
// now, marginalize out everything that is not variable j // now, marginalize out everything that is not variable j
GenericSequentialSolver<FactorType> solver(p_BC1C2); GenericSequentialSolver<FactorType> solver(p_BC1C2);
boost::shared_ptr<FactorGraph<FactorType> > result = solver.jointFactorGraph(js, function); boost::shared_ptr<FactorGraphOrdered<FactorType> > result = solver.jointFactorGraph(js, function);
// Undo the reduction // Undo the reduction
gttic(Undo_Reduce); gttic(Undo_Reduce);
@ -678,7 +678,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::jointBayesNet( typename BayesNetOrdered<CONDITIONAL>::shared_ptr BayesTreeOrdered<CONDITIONAL,CLIQUE>::jointBayesNet(
Index j1, Index j2, Eliminate function) const { Index j1, Index j2, Eliminate function) const {
// eliminate factor graph marginal to a Bayes net // eliminate factor graph marginal to a Bayes net
@ -688,7 +688,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::clear() { void BayesTreeOrdered<CONDITIONAL,CLIQUE>::clear() {
// Remove all nodes and clear the root pointer // Remove all nodes and clear the root pointer
nodes_.clear(); nodes_.clear();
root_.reset(); root_.reset();
@ -696,8 +696,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::removePath(sharedClique clique, void BayesTreeOrdered<CONDITIONAL,CLIQUE>::removePath(sharedClique clique,
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) { BayesNetOrdered<CONDITIONAL>& bn, typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::Cliques& orphans) {
// base case is NULL, if so we do nothing and return empties above // base case is NULL, if so we do nothing and return empties above
if (clique!=NULL) { if (clique!=NULL) {
@ -722,8 +722,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
template<class CONTAINER> template<class CONTAINER>
void BayesTree<CONDITIONAL,CLIQUE>::removeTop(const CONTAINER& keys, void BayesTreeOrdered<CONDITIONAL,CLIQUE>::removeTop(const CONTAINER& keys,
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) { BayesNetOrdered<CONDITIONAL>& bn, typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::Cliques& orphans) {
// process each key of the new factor // process each key of the new factor
BOOST_FOREACH(const Index& j, keys) { BOOST_FOREACH(const Index& j, keys) {
@ -746,7 +746,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::Cliques BayesTree<CONDITIONAL,CLIQUE>::removeSubtree( typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::Cliques BayesTreeOrdered<CONDITIONAL,CLIQUE>::removeSubtree(
const sharedClique& subtree) const sharedClique& subtree)
{ {
// Result clique list // Result clique list
@ -783,14 +783,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::cloneTo(This& newTree) const { void BayesTreeOrdered<CONDITIONAL,CLIQUE>::cloneTo(This& newTree) const {
if(root()) if(root())
cloneTo(newTree, root(), sharedClique()); cloneTo(newTree, root(), sharedClique());
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::cloneTo( void BayesTreeOrdered<CONDITIONAL,CLIQUE>::cloneTo(
This& newTree, const sharedClique& subtree, const sharedClique& parent) const { This& newTree, const sharedClique& subtree, const sharedClique& parent) const {
sharedClique newClique(subtree->clone()); sharedClique newClique(subtree->clone());
newTree.addClique(newClique, parent); newTree.addClique(newClique, parent);

418
gtsam/inference/BayesTreeOrdered.h Normal file
View File

@ -0,0 +1,418 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file BayesTree.h
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
* @author Frank Dellaert
*/
// \callgraph
#pragma once
#include <vector>
#include <stdexcept>
#include <deque>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/lexical_cast.hpp>
#include <gtsam/global_includes.h>
#include <gtsam/base/FastList.h>
#include <gtsam/inference/FactorGraphOrdered.h>
#include <gtsam/inference/BayesNetOrdered.h>
#include <gtsam/inference/BayesTreeCliqueBaseOrdered.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#include <gtsam/linear/VectorValuesOrdered.h>
namespace gtsam {
// Forward declaration of BayesTreeClique which is defined below BayesTree in this file
template<class CONDITIONAL> struct BayesTreeClique;
/**
* Bayes tree
* @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain,
* which could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional.
* @tparam CLIQUE The type of the clique data structure, defaults to BayesTreeClique, normally do not change this
* as it is only used when developing special versions of BayesTree, e.g. for ISAM2.
*
* \addtogroup Multifrontal
* \nosubgrouping
*/
template<class CONDITIONAL, class CLIQUE=BayesTreeClique<CONDITIONAL> >
class BayesTreeOrdered {
public:
typedef BayesTreeOrdered<CONDITIONAL, CLIQUE> This;
typedef boost::shared_ptr<BayesTreeOrdered<CONDITIONAL, CLIQUE> > shared_ptr;
typedef boost::shared_ptr<CONDITIONAL> sharedConditional;
typedef boost::shared_ptr<BayesNetOrdered<CONDITIONAL> > sharedBayesNet;
typedef CONDITIONAL ConditionalType;
typedef typename CONDITIONAL::FactorType FactorType;
typedef typename FactorGraphOrdered<FactorType>::Eliminate Eliminate;
typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
// typedef for shared pointers to cliques
typedef boost::shared_ptr<Clique> sharedClique;
// A convenience class for a list of shared cliques
struct Cliques : public FastList<sharedClique> {
void print(const std::string& s = "Cliques",
const IndexFormatter& indexFormatter = DefaultIndexFormatter) const;
bool equals(const Cliques& other, double tol = 1e-9) const;
};
/** clique statistics */
struct CliqueStats {
double avgConditionalSize;
std::size_t maxConditionalSize;
double avgSeparatorSize;
std::size_t maxSeparatorSize;
void print(const std::string& s = "") const ;
};
/** store all the sizes */
struct CliqueData {
std::vector<std::size_t> conditionalSizes;
std::vector<std::size_t> separatorSizes;
CliqueStats getStats() const;
};
/** Map from indices to Clique */
typedef std::vector<sharedClique> Nodes;
protected:
/** Map from indices to Clique */
Nodes nodes_;
/** Root clique */
sharedClique root_;
public:
/// @name Standard Constructors
/// @{
/** Create an empty Bayes Tree */
BayesTreeOrdered() {}
/** Create a Bayes Tree from a Bayes Net (requires CONDITIONAL is IndexConditional *or* CONDITIONAL::Combine) */
explicit BayesTreeOrdered(const BayesNetOrdered<CONDITIONAL>& bayesNet);
/** Copy constructor */
BayesTreeOrdered(const This& other);
/** Assignment operator */
This& operator=(const This& other);
/// @}
/// @name Advanced Constructors
/// @{
/**
* Create a Bayes Tree from a Bayes Net and some subtrees. The Bayes net corresponds to the
* new root clique and the subtrees are connected to the root clique.
*/
BayesTreeOrdered(const BayesNetOrdered<CONDITIONAL>& bayesNet, std::list<BayesTreeOrdered<CONDITIONAL,CLIQUE> > subtrees);
/** Destructor */
virtual ~BayesTreeOrdered() {}
/// @}
/// @name Testable
/// @{
/** check equality */
bool equals(const BayesTreeOrdered<CONDITIONAL,CLIQUE>& other, double tol = 1e-9) const;
/** print */
void print(const std::string& s = "",
const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const;
/// @}
/// @name Standard Interface
/// @{
/**
* Find parent clique of a conditional. It will look at all parents and
* return the one with the lowest index in the ordering.
*/
template<class CONTAINER>
Index findParentClique(const CONTAINER& parents) const;
/** number of cliques */
inline size_t size() const {
if(root_)
return root_->treeSize();
else
return 0;
}
/** number of nodes */
inline size_t nrNodes() const { return nodes_.size(); }
/** Check if there are any cliques in the tree */
inline bool empty() const {
return nodes_.empty();
}
/** return nodes */
const Nodes& nodes() const { return nodes_; }
/** return root clique */
const sharedClique& root() const { return root_; }
/** find the clique that contains the variable with Index j */
inline sharedClique operator[](Index j) const {
return nodes_.at(j);
}
/** alternate syntax for matlab: find the clique that contains the variable with Index j */
inline sharedClique clique(Index j) const {
return nodes_.at(j);
}
/** Gather data on all cliques */
CliqueData getCliqueData() const;
/** Collect number of cliques with cached separator marginals */
size_t numCachedSeparatorMarginals() const;
/** return marginal on any variable */
typename FactorType::shared_ptr marginalFactor(Index j, Eliminate function) const;
/**
* return marginal on any variable, as a Bayes Net
* NOTE: this function calls marginal, and then eliminates it into a Bayes Net
* This is more expensive than the above function
*/
typename BayesNetOrdered<CONDITIONAL>::shared_ptr marginalBayesNet(Index j, Eliminate function) const;
/**
* return joint on two variables
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
*/
typename FactorGraphOrdered<FactorType>::shared_ptr joint(Index j1, Index j2, Eliminate function) const;
/**
* return joint on two variables as a BayesNet
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
*/
typename BayesNetOrdered<CONDITIONAL>::shared_ptr jointBayesNet(Index j1, Index j2, Eliminate function) const;
/**
* Read only with side effects
*/
/** saves the Tree to a text file in GraphViz format */
void saveGraph(const std::string& s, const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const;
/// @}
/// @name Advanced Interface
/// @{
/** Access the root clique (non-const version) */
sharedClique& root() { return root_; }
/** Access the nodes (non-cost version) */
Nodes& nodes() { return nodes_; }
/** Remove all nodes */
void clear();
/** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */
void deleteCachedShortcuts() {
root_->deleteCachedShortcuts();
}
/** Apply a permutation to the full tree - also updates the nodes structure */
void permuteWithInverse(const Permutation& inversePermutation);
/**
* Remove path from clique to root and return that path as factors
* plus a list of orphaned subtree roots. Used in removeTop below.
*/
void removePath(sharedClique clique, BayesNetOrdered<CONDITIONAL>& bn, Cliques& orphans);
/**
* Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
* Factors and orphans are added to the in/out arguments.
*/
template<class CONTAINER>
void removeTop(const CONTAINER& indices, BayesNetOrdered<CONDITIONAL>& bn, Cliques& orphans);
/**
* Remove the requested subtree. */
Cliques removeSubtree(const sharedClique& subtree);
/**
* Hang a new subtree off of the existing tree. This finds the appropriate
* parent clique for the subtree (which may be the root), and updates the
* nodes index with the new cliques in the subtree. None of the frontal
* variables in the subtree may appear in the separators of the existing
* BayesTree.
*/
void insert(const sharedClique& subtree);
/** Insert a new conditional
* This function only applies for Symbolic case with IndexCondtional,
* We make it static so that it won't be compiled in GaussianConditional case.
* */
static void insert(BayesTreeOrdered<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional);
/**
* Insert a new clique corresponding to the given Bayes net.
* It is the caller's responsibility to decide whether the given Bayes net is a valid clique,
* i.e. all the variables (frontal and separator) are connected
*/
sharedClique insert(const sharedConditional& clique,
std::list<sharedClique>& children, bool isRootClique = false);
/**
* Create a clone of this object as a shared pointer
* Necessary for inheritance in matlab interface
*/
virtual shared_ptr clone() const {
return shared_ptr(new This(*this));
}
protected:
/** private helper method for saving the Tree to a text file in GraphViz format */
void saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter,
int parentnum = 0) const;
/** Gather data on a single clique */
void getCliqueData(CliqueData& stats, sharedClique clique) const;
/** remove a clique: warning, can result in a forest */
void removeClique(sharedClique clique);
/** add a clique (top down) */
sharedClique addClique(const sharedConditional& conditional, const sharedClique& parent_clique = sharedClique());
/** add a clique (top down) */
void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());
/** add a clique (bottom up) */
sharedClique addClique(const sharedConditional& conditional, std::list<sharedClique>& child_cliques);
/**
* Add a conditional to the front of a clique, i.e. a conditional whose
* parents are already in the clique or its separators. This function does
* not check for this condition, it just updates the data structures.
*/
static void addToCliqueFront(BayesTreeOrdered<CONDITIONAL,CLIQUE>& bayesTree,
const sharedConditional& conditional, const sharedClique& clique);
/** Fill the nodes index for a subtree */
void fillNodesIndex(const sharedClique& subtree);
/** Helper function to build a non-symbolic tree (e.g. Gaussian) using a
* symbolic tree, used in the BT(BN) constructor.
*/
void recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditionalOrdered> >& symbolic,
const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals,
const typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique& parent);
private:
/** deep copy to another tree */
void cloneTo(This& newTree) const;
/** deep copy to another tree */
void cloneTo(This& newTree, const sharedClique& subtree, const sharedClique& parent) const;
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(nodes_);
ar & BOOST_SERIALIZATION_NVP(root_);
}
/// @}
}; // BayesTree
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void _BayesTree_dim_adder(
std::vector<size_t>& dims,
const typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique& clique) {
if(clique) {
// Add dims from this clique
for(typename CONDITIONAL::const_iterator it = (*clique)->beginFrontals(); it != (*clique)->endFrontals(); ++it)
dims[*it] = (*clique)->dim(it);
// Traverse children
typedef typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& child, clique->children()) {
_BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dims, child);
}
}
}
/* ************************************************************************* */
template<class CONDITIONAL,class CLIQUE>
boost::shared_ptr<VectorValuesOrdered> allocateVectorValues(const BayesTreeOrdered<CONDITIONAL,CLIQUE>& bt) {
std::vector<size_t> dimensions(bt.nodes().size(), 0);
_BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dimensions, bt.root());
return boost::shared_ptr<VectorValuesOrdered>(new VectorValuesOrdered(dimensions));
}
/* ************************************************************************* */
/**
* A Clique in the tree is an incomplete Bayes net: the variables
* in the Bayes net are the frontal nodes, and the variables conditioned
* on are the separator. We also have pointers up and down the tree.
*
* Since our Conditional class already handles multiple frontal variables,
* this Clique contains exactly 1 conditional.
*
* This is the default clique type in a BayesTree, but some algorithms, like
* iSAM2 (see ISAM2Clique), use a different clique type in order to store
* extra data along with the clique.
*/
template<class CONDITIONAL>
struct BayesTreeClique : public BayesTreeCliqueBaseOrdered<BayesTreeClique<CONDITIONAL>, CONDITIONAL> {
public:
typedef CONDITIONAL ConditionalType;
typedef BayesTreeClique<CONDITIONAL> This;
typedef BayesTreeCliqueBaseOrdered<This, CONDITIONAL> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
BayesTreeClique() {}
BayesTreeClique(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {}
BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
};
} /// namespace gtsam
#include <gtsam/inference/BayesTreeOrdered-inl.h>
#include <gtsam/inference/BayesTreeCliqueBaseOrdered-inl.h>

259
gtsam/inference/BayesTreeUnordered.h
View File

@ -1,259 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file BayesTree.h
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
* @author Frank Dellaert
*/
// \callgraph
#pragma once
#include <vector>
#include <string>
#include <gtsam/base/types.h>
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastMap.h>
namespace gtsam {
// Forward declarations
template<class FACTOR> class FactorGraphUnordered;
/**
* Bayes tree
* @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain,
* which could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional.
* @tparam CLIQUE The type of the clique data structure, defaults to BayesTreeClique, normally do not change this
* as it is only used when developing special versions of BayesTree, e.g. for ISAM2.
*
* \addtogroup Multifrontal
* \nosubgrouping
*/
template<class CLIQUE>
class BayesTreeUnordered
{
protected:
typedef BayesTreeUnordered<CLIQUE> This;
typedef boost::shared_ptr<This> shared_ptr;
public:
typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
typedef boost::shared_ptr<Clique> sharedClique; ///< Shared pointer to a clique
typedef Clique Node; ///< Synonym for Clique (TODO: remove)
typedef sharedClique sharedNode; ///< Synonym for sharedClique (TODO: remove)
typedef typename CLIQUE::ConditionalType ConditionalType;
typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef typename CLIQUE::BayesNetType BayesNetType;
typedef boost::shared_ptr<BayesNetType> sharedBayesNet;
typedef typename CLIQUE::FactorType FactorType;
typedef boost::shared_ptr<FactorType> sharedFactor;
typedef typename CLIQUE::FactorGraphType FactorGraphType;
typedef boost::shared_ptr<FactorGraphType> sharedFactorGraph;
typedef typename FactorGraphType::Eliminate Eliminate;
typedef typename CLIQUE::EliminationTraits EliminationTraits;
/** A convenience class for a list of shared cliques */
typedef FastList<sharedClique> Cliques;
/** clique statistics */
struct CliqueStats {
double avgConditionalSize;
std::size_t maxConditionalSize;
double avgSeparatorSize;
std::size_t maxSeparatorSize;
void print(const std::string& s = "") const ;
};
/** store all the sizes */
struct CliqueData {
std::vector<std::size_t> conditionalSizes;
std::vector<std::size_t> separatorSizes;
CliqueStats getStats() const;
};
/** Map from keys to Clique */
typedef FastMap<Key, sharedClique> Nodes;
protected:
/** Map from indices to Clique */
Nodes nodes_;
/** Root cliques */
std::vector<sharedClique> roots_;
/// @name Standard Constructors
/// @{
/** Create an empty Bayes Tree */
BayesTreeUnordered() {}
/** Copy constructor */
BayesTreeUnordered(const This& other);
/// @}
/** Assignment operator */
This& operator=(const This& other);
/// @name Testable
/// @{
/** check equality */
bool equals(const This& other, double tol = 1e-9) const;
public:
/** print */
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @}
/// @name Standard Interface
/// @{
/** number of cliques */
size_t size() const;
/** Check if there are any cliques in the tree */
inline bool empty() const {
return nodes_.empty();
}
/** return nodes */
const Nodes& nodes() const { return nodes_; }
/** Access node by variable */
const sharedNode operator[](Key j) const { return nodes_.at(j); }
/** return root cliques */
const std::vector<sharedClique>& roots() const { return roots_; }
/** alternate syntax for matlab: find the clique that contains the variable with Index j */
const sharedClique& clique(Key j) const {
Nodes::const_iterator c = nodes_.find(j);
if(c == nodes_.end())
throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree");
else
return c->second;
}
/** Gather data on all cliques */
CliqueData getCliqueData() const;
/** Collect number of cliques with cached separator marginals */
size_t numCachedSeparatorMarginals() const;
/** Return marginal on any variable. Note that this actually returns a conditional, for which a
* solution may be directly obtained by calling .solve() on the returned object.
* Alternatively, it may be directly used as its factor base class. For example, for Gaussian
* systems, this returns a GaussianConditional, which inherits from JacobianFactor and
* GaussianFactor. */
sharedConditional marginalFactor(Key j, const Eliminate& function = EliminationTraits::DefaultEliminate) const;
/**
* return joint on two variables
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
*/
sharedFactorGraph joint(Index j1, Index j2, const Eliminate& function = EliminationTraits::DefaultEliminate) const;
/**
* return joint on two variables as a BayesNet
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
*/
sharedBayesNet jointBayesNet(Index j1, Index j2, const Eliminate& function = EliminationTraits::DefaultEliminate) const;
/**
* Read only with side effects
*/
/** saves the Tree to a text file in GraphViz format */
void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @}
/// @name Advanced Interface
/// @{
/**
* Find parent clique of a conditional. It will look at all parents and
* return the one with the lowest index in the ordering.
*/
template<class CONTAINER>
Index findParentClique(const CONTAINER& parents) const;
/** Remove all nodes */
void clear();
/** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */
void deleteCachedShortcuts();
/**
* Remove path from clique to root and return that path as factors
* plus a list of orphaned subtree roots. Used in removeTop below.
*/
void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans);
/**
* Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
* Factors and orphans are added to the in/out arguments.
*/
void removeTop(const std::vector<Key>& keys, BayesNetType& bn, Cliques& orphans);
/**
* Remove the requested subtree. */
Cliques removeSubtree(const sharedClique& subtree);
/** Insert a new subtree with known parent clique. This function does not check that the
* specified parent is the correct parent. This function updates all of the internal data
* structures associated with adding a subtree, such as populating the nodes index. */
void insertRoot(const sharedClique& subtree);
/** add a clique (top down) */
void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());
/** Add all cliques in this BayesTree to the specified factor graph */
void addFactorsToGraph(FactorGraphUnordered<FactorType>& graph) const;
protected:
/** private helper method for saving the Tree to a text file in GraphViz format */
void saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter,
int parentnum = 0) const;
/** Gather data on a single clique */
void getCliqueData(CliqueData& stats, sharedClique clique) const;
/** remove a clique: warning, can result in a forest */
void removeClique(sharedClique clique);
/** add a clique (bottom up) */
sharedClique addClique(const sharedConditional& conditional, std::list<sharedClique>& child_cliques);
/** Fill the nodes index for a subtree */
void fillNodesIndex(const sharedClique& subtree);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(nodes_);
ar & BOOST_SERIALIZATION_NVP(root_);
}
/// @}
}; // BayesTree
} /// namespace gtsam

20
gtsam/inference/ClusterTree-inl.h → gtsam/inference/ClusterTreeOrdered-inl.h
View File

@ -22,7 +22,7 @@
#include <iostream> #include <iostream>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <gtsam/inference/ClusterTree.h> #include <gtsam/inference/ClusterTreeOrdered.h>
namespace gtsam { namespace gtsam {
@ -31,32 +31,32 @@ namespace gtsam {
* ************************************************************************* */ * ************************************************************************* */
template<class FG> template<class FG>
template<class Iterator> template<class Iterator>
ClusterTree<FG>::Cluster::Cluster(const FG& fg, Index key, Iterator firstSeparator, Iterator lastSeparator) : ClusterTreeOrdered<FG>::Cluster::Cluster(const FG& fg, Index key, Iterator firstSeparator, Iterator lastSeparator) :
FG(fg), frontal(1, key), separator(firstSeparator, lastSeparator) {} FG(fg), frontal(1, key), separator(firstSeparator, lastSeparator) {}
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG> template<class FG>
template<typename FRONTALIT, typename SEPARATORIT> template<typename FRONTALIT, typename SEPARATORIT>
ClusterTree<FG>::Cluster::Cluster( ClusterTreeOrdered<FG>::Cluster::Cluster(
const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) : const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) :
FG(fg), frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {} FG(fg), frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {}
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG> template<class FG>
template<typename FRONTALIT, typename SEPARATORIT> template<typename FRONTALIT, typename SEPARATORIT>
ClusterTree<FG>::Cluster::Cluster( ClusterTreeOrdered<FG>::Cluster::Cluster(
FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) : FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) :
frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {} frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {}
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG> template<class FG>
void ClusterTree<FG>::Cluster::addChild(typename ClusterTree<FG>::Cluster::shared_ptr child) { void ClusterTreeOrdered<FG>::Cluster::addChild(typename ClusterTreeOrdered<FG>::Cluster::shared_ptr child) {
children_.push_back(child); children_.push_back(child);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG> template<class FG>
bool ClusterTree<FG>::Cluster::equals(const Cluster& other) const { bool ClusterTreeOrdered<FG>::Cluster::equals(const Cluster& other) const {
if (frontal != other.frontal) return false; if (frontal != other.frontal) return false;
if (separator != other.separator) return false; if (separator != other.separator) return false;
if (children_.size() != other.children_.size()) return false; if (children_.size() != other.children_.size()) return false;
@ -71,7 +71,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG> template<class FG>
void ClusterTree<FG>::Cluster::print(const std::string& indent, void ClusterTreeOrdered<FG>::Cluster::print(const std::string& indent,
const IndexFormatter& formatter) const { const IndexFormatter& formatter) const {
std::cout << indent; std::cout << indent;
BOOST_FOREACH(const Index key, frontal) BOOST_FOREACH(const Index key, frontal)
@ -84,7 +84,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG> template<class FG>
void ClusterTree<FG>::Cluster::printTree(const std::string& indent, void ClusterTreeOrdered<FG>::Cluster::printTree(const std::string& indent,
const IndexFormatter& formatter) const { const IndexFormatter& formatter) const {
print(indent, formatter); print(indent, formatter);
BOOST_FOREACH(const shared_ptr& child, children_) BOOST_FOREACH(const shared_ptr& child, children_)
@ -95,7 +95,7 @@ namespace gtsam {
* ClusterTree * ClusterTree
* ************************************************************************* */ * ************************************************************************* */
template<class FG> template<class FG>
void ClusterTree<FG>::print(const std::string& str, void ClusterTreeOrdered<FG>::print(const std::string& str,
const IndexFormatter& formatter) const { const IndexFormatter& formatter) const {
std::cout << str << std::endl; std::cout << str << std::endl;
if (root_) root_->printTree("", formatter); if (root_) root_->printTree("", formatter);
@ -103,7 +103,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG> template<class FG>
bool ClusterTree<FG>::equals(const ClusterTree<FG>& other, double tol) const { bool ClusterTreeOrdered<FG>::equals(const ClusterTreeOrdered<FG>& other, double tol) const {
if (!root_ && !other.root_) return true; if (!root_ && !other.root_) return true;
if (!root_ || !other.root_) return false; if (!root_ || !other.root_) return false;
return root_->equals(*other.root_); return root_->equals(*other.root_);

8
gtsam/inference/ClusterTree.h → gtsam/inference/ClusterTreeOrdered.h
View File

@ -38,7 +38,7 @@ namespace gtsam {
* \nosubgrouping * \nosubgrouping
*/ */
template <class FG> template <class FG>
class ClusterTree { class ClusterTreeOrdered {
public: public:
// Access to factor types // Access to factor types
typedef typename FG::KeyType KeyType; typedef typename FG::KeyType KeyType;
@ -113,7 +113,7 @@ namespace gtsam {
/// @{ /// @{
/// constructor of empty tree /// constructor of empty tree
ClusterTree() {} ClusterTreeOrdered() {}
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
@ -130,7 +130,7 @@ namespace gtsam {
void print(const std::string& str="", const IndexFormatter& formatter = DefaultIndexFormatter) const; void print(const std::string& str="", const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** check equality */ /** check equality */
bool equals(const ClusterTree<FG>& other, double tol = 1e-9) const; bool equals(const ClusterTreeOrdered<FG>& other, double tol = 1e-9) const;
/// @} /// @}
@ -138,4 +138,4 @@ namespace gtsam {
} // namespace gtsam } // namespace gtsam
#include <gtsam/inference/ClusterTree-inl.h> #include <gtsam/inference/ClusterTreeOrdered-inl.h>

6
gtsam/inference/ConditionalUnordered-inst.h → gtsam/inference/Conditional-inst.h
View File

@ -21,13 +21,13 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <iostream> #include <iostream>
#include <gtsam/inference/ConditionalUnordered.h> #include <gtsam/inference/Conditional.h>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR, class DERIVEDFACTOR> template<class FACTOR, class DERIVEDFACTOR>
void ConditionalUnordered<FACTOR,DERIVEDFACTOR>::print(const std::string& s, const KeyFormatter& formatter) const { void Conditional<FACTOR,DERIVEDFACTOR>::print(const std::string& s, const KeyFormatter& formatter) const {
std::cout << s << " P("; std::cout << s << " P(";
BOOST_FOREACH(Key key, frontals()) BOOST_FOREACH(Key key, frontals())
std::cout << " " << formatter(key); std::cout << " " << formatter(key);
@ -40,7 +40,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR, class DERIVEDFACTOR> template<class FACTOR, class DERIVEDFACTOR>
bool ConditionalUnordered<FACTOR,DERIVEDFACTOR>::equals(const This& c, double tol = 1e-9) const bool Conditional<FACTOR,DERIVEDFACTOR>::equals(const This& c, double tol = 1e-9) const
{ {
return nrFrontals_ == c.nrFrontals_; return nrFrontals_ == c.nrFrontals_;
} }

413
gtsam/inference/Conditional.h
View File

@ -1,266 +1,147 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file Conditional.h * @file Conditional.h
* @brief Base class for conditional densities * @brief Base class for conditional densities
* @author Frank Dellaert * @author Frank Dellaert
*/ */
// \callgraph // \callgraph
#pragma once #pragma once
#include <gtsam/inference/Factor.h> #include <boost/range.hpp>
#include <boost/range.hpp> #include <gtsam/inference/Key.h>
#include <iostream> namespace gtsam {
#include <list>
/**
namespace gtsam { * Base class for conditional densities, templated on KEY type. This class
* provides storage for the keys involved in a conditional, and iterators and
/** * access to the frontal and separator keys.
* Base class for conditional densities, templated on KEY type. This class *
* provides storage for the keys involved in a conditional, and iterators and * Derived classes *must* redefine the Factor and shared_ptr typedefs to refer
* access to the frontal and separator keys. * to the associated factor type and shared_ptr type of the derived class. See
* * IndexConditional and GaussianConditional for examples.
* Derived classes *must* redefine the Factor and shared_ptr typedefs to refer * \nosubgrouping
* to the associated factor type and shared_ptr type of the derived class. See */
* IndexConditional and GaussianConditional for examples. template<class FACTOR, class DERIVEDCONDITIONAL>
* \nosubgrouping class Conditional
*/ {
template<typename KEY> protected:
class Conditional: public gtsam::Factor<KEY> { /** The first nrFrontal variables are frontal and the rest are parents. */
size_t nrFrontals_;
private:
private:
/** Create keys by adding key in front */ /// Typedef to this class
template<typename ITERATOR> typedef Conditional<FACTOR,DERIVEDCONDITIONAL> This;
static std::vector<KEY> MakeKeys(KEY key, ITERATOR firstParent,
ITERATOR lastParent) { public:
std::vector<KeyType> keys((lastParent - firstParent) + 1); /** View of the frontal keys (call frontals()) */
std::copy(firstParent, lastParent, keys.begin() + 1); typedef boost::iterator_range<typename FACTOR::const_iterator> Frontals;
keys[0] = key;
return keys; /** View of the separator keys (call parents()) */
} typedef boost::iterator_range<typename FACTOR::const_iterator> Parents;
protected: protected:
/// @name Standard Constructors
/** The first nFrontal variables are frontal and the rest are parents. */ /// @{
size_t nrFrontals_;
/** Empty Constructor to make serialization possible */
// Calls the base class assertInvariants, which checks for unique keys Conditional() : nrFrontals_(0) {}
void assertInvariants() const {
Factor<KEY>::assertInvariants(); /** Constructor */
} Conditional(size_t nrFrontals) : nrFrontals_(nrFrontals) {}
public: /// @}
/// @name Testable
typedef KEY KeyType; /// @{
typedef Conditional<KeyType> This;
typedef Factor<KeyType> Base; /** print with optional formatter */
void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const;
/**
* Typedef to the factor type that produces this conditional and that this /** check equality */
* conditional can be converted to using a factor constructor. Derived bool equals(const This& c, double tol = 1e-9) const;
* classes must redefine this.
*/ /// @}
typedef gtsam::Factor<KeyType> FactorType;
public:
/** A shared_ptr to this class. Derived classes must redefine this. */ /// @name Standard Interface
typedef boost::shared_ptr<This> shared_ptr; /// @{
/** Iterator over keys */ /** return the number of frontals */
typedef typename FactorType::iterator iterator; size_t nrFrontals() const { return nrFrontals_; }
/** Const iterator over keys */ /** return the number of parents */
typedef typename FactorType::const_iterator const_iterator; size_t nrParents() const { return asFactor().size() - nrFrontals_; }
/** View of the frontal keys (call frontals()) */ /** Convenience function to get the first frontal key */
typedef boost::iterator_range<const_iterator> Frontals; Key firstFrontalKey() const {
if(nrFrontals_ > 0)
/** View of the separator keys (call parents()) */ return asFactor().front();
typedef boost::iterator_range<const_iterator> Parents; else
throw std::invalid_argument("Requested Conditional::firstFrontalKey from a conditional with zero frontal keys");
/// @name Standard Constructors }
/// @{
/** return a view of the frontal keys */
/** Empty Constructor to make serialization possible */ Frontals frontals() const { return boost::make_iterator_range(beginFrontals(), endFrontals()); }
Conditional() :
nrFrontals_(0) { /** return a view of the parent keys */
assertInvariants(); Parents parents() const { return boost::make_iterator_range(beginParents(), endParents()); }
}
/** Iterator pointing to first frontal key. */
/** No parents */ typename FACTOR::const_iterator beginFrontals() const { return asFactor().begin(); }
Conditional(KeyType key) :
FactorType(key), nrFrontals_(1) { /** Iterator pointing past the last frontal key. */
assertInvariants(); typename FACTOR::const_iterator endFrontals() const { return asFactor().begin() + nrFrontals_; }
}
/** Iterator pointing to the first parent key. */
/** Single parent */ typename FACTOR::const_iterator beginParents() const { return endFrontals(); }
Conditional(KeyType key, KeyType parent) :
FactorType(key, parent), nrFrontals_(1) { /** Iterator pointing past the last parent key. */
assertInvariants(); typename FACTOR::const_iterator endParents() const { return asFactor().end(); }
}
/// @}
/** Two parents */ /// @name Advanced Interface
Conditional(KeyType key, KeyType parent1, KeyType parent2) : /// @{
FactorType(key, parent1, parent2), nrFrontals_(1) {
assertInvariants(); /** Mutable iterator pointing to first frontal key. */
} typename FACTOR::iterator beginFrontals() { return asFactor().begin(); }
/** Three parents */ /** Mutable iterator pointing past the last frontal key. */
Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) : typename FACTOR::iterator endFrontals() { return asFactor().begin() + nrFrontals_; }
FactorType(key, parent1, parent2, parent3), nrFrontals_(1) {
assertInvariants(); /** Mutable iterator pointing to the first parent key. */
} typename FACTOR::iterator beginParents() { return asFactor().begin() + nrFrontals_; }
/// @} /** Mutable iterator pointing past the last parent key. */
/// @name Advanced Constructors typename FACTOR::iterator endParents() { return asFactor().end(); }
/// @{
private:
/** Constructor from a frontal variable and a vector of parents */ // Cast to factor type (non-const) (casts down to derived conditional type, then up to factor type)
Conditional(KeyType key, const std::vector<KeyType>& parents) : FACTOR& asFactor() { return static_cast<FACTOR&>(static_cast<DERIVEDCONDITIONAL&>(*this)); }
FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(
1) { // Cast to derived type (const) (casts down to derived conditional type, then up to factor type)
assertInvariants(); const FACTOR& asFactor() const { return static_cast<const FACTOR&>(static_cast<const DERIVEDCONDITIONAL&>(*this)); }
}
/** Serialization function */
/** Constructor from keys and nr of frontal variables */ friend class boost::serialization::access;
Conditional(const std::vector<Index>& keys, size_t nrFrontals) : template<class ARCHIVE>
FactorType(keys), nrFrontals_(nrFrontals) { void serialize(ARCHIVE & ar, const unsigned int version) {
assertInvariants(); ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
} }
/** Constructor from keys and nr of frontal variables */ /// @}
Conditional(const std::list<Index>& keys, size_t nrFrontals) :
FactorType(keys.begin(),keys.end()), nrFrontals_(nrFrontals) { };
assertInvariants();
} } // gtsam
/// @}
/// @name Testable
/// @{
/** print with optional formatter */
void print(const std::string& s = "Conditional",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** check equality */
template<class DERIVED>
bool equals(const DERIVED& c, double tol = 1e-9) const {
return nrFrontals_ == c.nrFrontals_ && FactorType::equals(c, tol);
}
/// @}
/// @name Standard Interface
/// @{
/** return the number of frontals */
size_t nrFrontals() const {
return nrFrontals_;
}
/** return the number of parents */
size_t nrParents() const {
return FactorType::size() - nrFrontals_;
}
/** Special accessor when there is only one frontal variable. */
KeyType firstFrontalKey() const {
assert(nrFrontals_>0);
return FactorType::front();
}
KeyType lastFrontalKey() const {
assert(nrFrontals_>0);
return *(endFrontals() - 1);
}
/** return a view of the frontal keys */
Frontals frontals() const {
return boost::make_iterator_range(beginFrontals(), endFrontals());
}
/** return a view of the parent keys */
Parents parents() const {
return boost::make_iterator_range(beginParents(), endParents());
}
/** Iterators over frontal and parent variables. */
const_iterator beginFrontals() const {
return FactorType::begin();
} ///<TODO: comment
const_iterator endFrontals() const {
return FactorType::begin() + nrFrontals_;
} ///<TODO: comment
const_iterator beginParents() const {
return FactorType::begin() + nrFrontals_;
} ///<TODO: comment
const_iterator endParents() const {
return FactorType::end();
} ///<TODO: comment
/// @}
/// @name Advanced Interface
/// @{
/** Mutable iterators and accessors */
iterator beginFrontals() {
return FactorType::begin();
} ///<TODO: comment
iterator endFrontals() {
return FactorType::begin() + nrFrontals_;
} ///<TODO: comment
iterator beginParents() {
return FactorType::begin() + nrFrontals_;
} ///<TODO: comment
iterator endParents() {
return FactorType::end();
} ///<TODO: comment
///TODO: comment
boost::iterator_range<iterator> frontals() {
return boost::make_iterator_range(beginFrontals(), endFrontals());
}
///TODO: comment
boost::iterator_range<iterator> parents() {
return boost::make_iterator_range(beginParents(), endParents());
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
}
/// @}
};
/* ************************************************************************* */
template<typename KEY>
void Conditional<KEY>::print(const std::string& s,
const IndexFormatter& formatter) const {
std::cout << s << " P(";
BOOST_FOREACH(KeyType key, frontals())
std::cout << " " << formatter(key);
if (nrParents() > 0)
std::cout << " |";
BOOST_FOREACH(KeyType parent, parents())
std::cout << " " << formatter(parent);
std::cout << ")" << std::endl;
}
} // gtsam

266
gtsam/inference/ConditionalOrdered.h Normal file
View File

@ -0,0 +1,266 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Conditional.h
* @brief Base class for conditional densities
* @author Frank Dellaert
*/
// \callgraph
#pragma once
#include <gtsam/inference/FactorOrdered.h>
#include <boost/range.hpp>
#include <iostream>
#include <list>
namespace gtsam {
/**
* Base class for conditional densities, templated on KEY type. This class
* provides storage for the keys involved in a conditional, and iterators and
* access to the frontal and separator keys.
*
* Derived classes *must* redefine the Factor and shared_ptr typedefs to refer
* to the associated factor type and shared_ptr type of the derived class. See
* IndexConditional and GaussianConditional for examples.
* \nosubgrouping
*/
template<typename KEY>
class ConditionalOrdered: public gtsam::FactorOrdered<KEY> {
private:
/** Create keys by adding key in front */
template<typename ITERATOR>
static std::vector<KEY> MakeKeys(KEY key, ITERATOR firstParent,
ITERATOR lastParent) {
std::vector<KeyType> keys((lastParent - firstParent) + 1);
std::copy(firstParent, lastParent, keys.begin() + 1);
keys[0] = key;
return keys;
}
protected:
/** The first nFrontal variables are frontal and the rest are parents. */
size_t nrFrontals_;
// Calls the base class assertInvariants, which checks for unique keys
void assertInvariants() const {
FactorOrdered<KEY>::assertInvariants();
}
public:
typedef KEY KeyType;
typedef ConditionalOrdered<KeyType> This;
typedef FactorOrdered<KeyType> Base;
/**
* Typedef to the factor type that produces this conditional and that this
* conditional can be converted to using a factor constructor. Derived
* classes must redefine this.
*/
typedef gtsam::FactorOrdered<KeyType> FactorType;
/** A shared_ptr to this class. Derived classes must redefine this. */
typedef boost::shared_ptr<This> shared_ptr;
/** Iterator over keys */
typedef typename FactorType::iterator iterator;
/** Const iterator over keys */
typedef typename FactorType::const_iterator const_iterator;
/** View of the frontal keys (call frontals()) */
typedef boost::iterator_range<const_iterator> Frontals;
/** View of the separator keys (call parents()) */
typedef boost::iterator_range<const_iterator> Parents;
/// @name Standard Constructors
/// @{
/** Empty Constructor to make serialization possible */
ConditionalOrdered() :
nrFrontals_(0) {
assertInvariants();
}
/** No parents */
ConditionalOrdered(KeyType key) :
FactorType(key), nrFrontals_(1) {
assertInvariants();
}
/** Single parent */
ConditionalOrdered(KeyType key, KeyType parent) :
FactorType(key, parent), nrFrontals_(1) {
assertInvariants();
}
/** Two parents */
ConditionalOrdered(KeyType key, KeyType parent1, KeyType parent2) :
FactorType(key, parent1, parent2), nrFrontals_(1) {
assertInvariants();
}
/** Three parents */
ConditionalOrdered(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) :
FactorType(key, parent1, parent2, parent3), nrFrontals_(1) {
assertInvariants();
}
/// @}
/// @name Advanced Constructors
/// @{
/** Constructor from a frontal variable and a vector of parents */
ConditionalOrdered(KeyType key, const std::vector<KeyType>& parents) :
FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(
1) {
assertInvariants();
}
/** Constructor from keys and nr of frontal variables */
ConditionalOrdered(const std::vector<Index>& keys, size_t nrFrontals) :
FactorType(keys), nrFrontals_(nrFrontals) {
assertInvariants();
}
/** Constructor from keys and nr of frontal variables */
ConditionalOrdered(const std::list<Index>& keys, size_t nrFrontals) :
FactorType(keys.begin(),keys.end()), nrFrontals_(nrFrontals) {
assertInvariants();
}
/// @}
/// @name Testable
/// @{
/** print with optional formatter */
void print(const std::string& s = "Conditional",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** check equality */
template<class DERIVED>
bool equals(const DERIVED& c, double tol = 1e-9) const {
return nrFrontals_ == c.nrFrontals_ && FactorType::equals(c, tol);
}
/// @}
/// @name Standard Interface
/// @{
/** return the number of frontals */
size_t nrFrontals() const {
return nrFrontals_;
}
/** return the number of parents */
size_t nrParents() const {
return FactorType::size() - nrFrontals_;
}
/** Special accessor when there is only one frontal variable. */
KeyType firstFrontalKey() const {
assert(nrFrontals_>0);
return FactorType::front();
}
KeyType lastFrontalKey() const {
assert(nrFrontals_>0);
return *(endFrontals() - 1);
}
/** return a view of the frontal keys */
Frontals frontals() const {
return boost::make_iterator_range(beginFrontals(), endFrontals());
}
/** return a view of the parent keys */
Parents parents() const {
return boost::make_iterator_range(beginParents(), endParents());
}
/** Iterators over frontal and parent variables. */
const_iterator beginFrontals() const {
return FactorType::begin();
} ///<TODO: comment
const_iterator endFrontals() const {
return FactorType::begin() + nrFrontals_;
} ///<TODO: comment
const_iterator beginParents() const {
return FactorType::begin() + nrFrontals_;
} ///<TODO: comment
const_iterator endParents() const {
return FactorType::end();
} ///<TODO: comment
/// @}
/// @name Advanced Interface
/// @{
/** Mutable iterators and accessors */
iterator beginFrontals() {
return FactorType::begin();
} ///<TODO: comment
iterator endFrontals() {
return FactorType::begin() + nrFrontals_;
} ///<TODO: comment
iterator beginParents() {
return FactorType::begin() + nrFrontals_;
} ///<TODO: comment
iterator endParents() {
return FactorType::end();
} ///<TODO: comment
///TODO: comment
boost::iterator_range<iterator> frontals() {
return boost::make_iterator_range(beginFrontals(), endFrontals());
}
///TODO: comment
boost::iterator_range<iterator> parents() {
return boost::make_iterator_range(beginParents(), endParents());
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
}
/// @}
};
/* ************************************************************************* */
template<typename KEY>
void ConditionalOrdered<KEY>::print(const std::string& s,
const IndexFormatter& formatter) const {
std::cout << s << " P(";
BOOST_FOREACH(KeyType key, frontals())
std::cout << " " << formatter(key);
if (nrParents() > 0)
std::cout << " |";
BOOST_FOREACH(KeyType parent, parents())
std::cout << " " << formatter(parent);
std::cout << ")" << std::endl;
}
} // gtsam

147
gtsam/inference/ConditionalUnordered.h
View File

@ -1,147 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Conditional.h
* @brief Base class for conditional densities
* @author Frank Dellaert
*/
// \callgraph
#pragma once
#include <boost/range.hpp>
#include <gtsam/inference/Key.h>
namespace gtsam {
/**
* Base class for conditional densities, templated on KEY type. This class
* provides storage for the keys involved in a conditional, and iterators and
* access to the frontal and separator keys.
*
* Derived classes *must* redefine the Factor and shared_ptr typedefs to refer
* to the associated factor type and shared_ptr type of the derived class. See
* IndexConditional and GaussianConditional for examples.
* \nosubgrouping
*/
template<class FACTOR, class DERIVEDCONDITIONAL>
class ConditionalUnordered
{
protected:
/** The first nrFrontal variables are frontal and the rest are parents. */
size_t nrFrontals_;
private:
/// Typedef to this class
typedef ConditionalUnordered<FACTOR,DERIVEDCONDITIONAL> This;
public:
/** View of the frontal keys (call frontals()) */
typedef boost::iterator_range<typename FACTOR::const_iterator> Frontals;
/** View of the separator keys (call parents()) */
typedef boost::iterator_range<typename FACTOR::const_iterator> Parents;
protected:
/// @name Standard Constructors
/// @{
/** Empty Constructor to make serialization possible */
ConditionalUnordered() : nrFrontals_(0) {}
/** Constructor */
ConditionalUnordered(size_t nrFrontals) : nrFrontals_(nrFrontals) {}
/// @}
/// @name Testable
/// @{
/** print with optional formatter */
void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const;
/** check equality */
bool equals(const This& c, double tol = 1e-9) const;
/// @}
public:
/// @name Standard Interface
/// @{
/** return the number of frontals */
size_t nrFrontals() const { return nrFrontals_; }
/** return the number of parents */
size_t nrParents() const { return asFactor().size() - nrFrontals_; }
/** Convenience function to get the first frontal key */
Key firstFrontalKey() const {
if(nrFrontals_ > 0)
return asFactor().front();
else
throw std::invalid_argument("Requested Conditional::firstFrontalKey from a conditional with zero frontal keys");
}
/** return a view of the frontal keys */
Frontals frontals() const { return boost::make_iterator_range(beginFrontals(), endFrontals()); }
/** return a view of the parent keys */
Parents parents() const { return boost::make_iterator_range(beginParents(), endParents()); }
/** Iterator pointing to first frontal key. */
typename FACTOR::const_iterator beginFrontals() const { return asFactor().begin(); }
/** Iterator pointing past the last frontal key. */
typename FACTOR::const_iterator endFrontals() const { return asFactor().begin() + nrFrontals_; }
/** Iterator pointing to the first parent key. */
typename FACTOR::const_iterator beginParents() const { return endFrontals(); }
/** Iterator pointing past the last parent key. */
typename FACTOR::const_iterator endParents() const { return asFactor().end(); }
/// @}
/// @name Advanced Interface
/// @{
/** Mutable iterator pointing to first frontal key. */
typename FACTOR::iterator beginFrontals() { return asFactor().begin(); }
/** Mutable iterator pointing past the last frontal key. */
typename FACTOR::iterator endFrontals() { return asFactor().begin() + nrFrontals_; }
/** Mutable iterator pointing to the first parent key. */
typename FACTOR::iterator beginParents() { return asFactor().begin() + nrFrontals_; }
/** Mutable iterator pointing past the last parent key. */
typename FACTOR::iterator endParents() { return asFactor().end(); }
private:
// Cast to factor type (non-const) (casts down to derived conditional type, then up to factor type)
FACTOR& asFactor() { return static_cast<FACTOR&>(static_cast<DERIVEDCONDITIONAL&>(*this)); }
// Cast to derived type (const) (casts down to derived conditional type, then up to factor type)
const FACTOR& asFactor() const { return static_cast<const FACTOR&>(static_cast<const DERIVEDCONDITIONAL&>(*this)); }
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
}
/// @}
};
} // gtsam

34
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -44,13 +44,13 @@ namespace gtsam {
// If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
// for no variable index first so that it's always computed if we need to call COLAMD because // for no variable index first so that it's always computed if we need to call COLAMD because
// no Ordering is provided. // no Ordering is provided.
return eliminateSequential(ordering, function, VariableIndexUnordered(asDerived())); return eliminateSequential(ordering, function, VariableIndex(asDerived()));
} }
else /*if(!ordering)*/ { else /*if(!ordering)*/ {
// If no Ordering provided, compute one and call this function again. We are guaranteed to // If no Ordering provided, compute one and call this function again. We are guaranteed to
// have a VariableIndex already here because we computed one if needed in the previous 'else' // have a VariableIndex already here because we computed one if needed in the previous 'else'
// block. // block.
return eliminateSequential(OrderingUnordered::COLAMD(*variableIndex), function); return eliminateSequential(Ordering::COLAMD(*variableIndex), function);
} }
} }
@ -75,13 +75,13 @@ namespace gtsam {
// If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
// for no variable index first so that it's always computed if we need to call COLAMD because // for no variable index first so that it's always computed if we need to call COLAMD because
// no Ordering is provided. // no Ordering is provided.
return eliminateMultifrontal(ordering, function, VariableIndexUnordered(asDerived())); return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()));
} }
else /*if(!ordering)*/ { else /*if(!ordering)*/ {
// If no Ordering provided, compute one and call this function again. We are guaranteed to // If no Ordering provided, compute one and call this function again. We are guaranteed to
// have a VariableIndex already here because we computed one if needed in the previous 'else' // have a VariableIndex already here because we computed one if needed in the previous 'else'
// block. // block.
return eliminateMultifrontal(OrderingUnordered::COLAMD(*variableIndex), function); return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function);
} }
} }
@ -89,7 +89,7 @@ namespace gtsam {
template<class FACTORGRAPH> template<class FACTORGRAPH>
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> > std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential( EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
const OrderingUnordered& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
{ {
if(variableIndex) { if(variableIndex) {
gttic(eliminatePartialSequential); gttic(eliminatePartialSequential);
@ -97,7 +97,7 @@ namespace gtsam {
return EliminationTreeType(asDerived(), *variableIndex, ordering).eliminate(function); return EliminationTreeType(asDerived(), *variableIndex, ordering).eliminate(function);
} else { } else {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
return eliminatePartialSequential(ordering, function, VariableIndexUnordered(asDerived())); return eliminatePartialSequential(ordering, function, VariableIndex(asDerived()));
} }
} }
@ -105,7 +105,7 @@ namespace gtsam {
template<class FACTORGRAPH> template<class FACTORGRAPH>
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> > std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal( EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
const OrderingUnordered& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
{ {
if(variableIndex) { if(variableIndex) {
gttic(eliminatePartialMultifrontal); gttic(eliminatePartialMultifrontal);
@ -113,7 +113,7 @@ namespace gtsam {
return JunctionTreeType(EliminationTreeType(asDerived(), *variableIndex, ordering)).eliminate(function); return JunctionTreeType(EliminationTreeType(asDerived(), *variableIndex, ordering)).eliminate(function);
} else { } else {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
return eliminatePartialMultifrontal(ordering, function, VariableIndexUnordered(asDerived())); return eliminatePartialMultifrontal(ordering, function, VariableIndex(asDerived()));
} }
} }
@ -121,7 +121,7 @@ namespace gtsam {
template<class FACTORGRAPH> template<class FACTORGRAPH>
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType> boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet( EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
boost::variant<const OrderingUnordered&, const std::vector<Key>&> variables, boost::variant<const Ordering&, const std::vector<Key>&> variables,
OptionalOrdering marginalizedVariableOrdering, OptionalOrdering marginalizedVariableOrdering,
const Eliminate& function = EliminationTraits::DefaultEliminate, const Eliminate& function = EliminationTraits::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const OptionalVariableIndex variableIndex = boost::none) const
@ -136,7 +136,7 @@ namespace gtsam {
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > eliminated = std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > eliminated =
eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex); eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex);
if(const OrderingUnordered* varsAsOrdering = boost::get<const OrderingUnordered&>(&variables)) if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
{ {
// An ordering was also provided for the unmarginalized variables, so we can also // An ordering was also provided for the unmarginalized variables, so we can also
// eliminate them in the order requested. // eliminate them in the order requested.
@ -152,25 +152,25 @@ namespace gtsam {
{ {
// No ordering was provided for the marginalized variables, so order them using constrained // No ordering was provided for the marginalized variables, so order them using constrained
// COLAMD. // COLAMD.
bool unmarginalizedAreOrdered = (boost::get<const OrderingUnordered&>(&variables) != 0); bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
const std::vector<Key>* variablesOrOrdering = const std::vector<Key>* variablesOrOrdering =
unmarginalizedAreOrdered ? unmarginalizedAreOrdered ?
boost::get<const OrderingUnordered&>(&variables) : boost::get<const std::vector<Key>&>(&variables); boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);
OrderingUnordered totalOrdering = Ordering totalOrdering =
OrderingUnordered::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); Ordering::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
// Split up ordering // Split up ordering
const size_t nVars = variablesOrOrdering->size(); const size_t nVars = variablesOrOrdering->size();
OrderingUnordered marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars); Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
OrderingUnordered marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
// Call this function again with the computed orderings // Call this function again with the computed orderings
return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
} }
} else { } else {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, VariableIndexUnordered(asDerived())); return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, VariableIndex(asDerived()));
} }
} }

18
gtsam/inference/EliminateableFactorGraph.h
View File

@ -22,8 +22,8 @@
#include <boost/function.hpp> #include <boost/function.hpp>
#include <boost/variant.hpp> #include <boost/variant.hpp>
#include <gtsam/inference/OrderingUnordered.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/inference/VariableIndexUnordered.h> #include <gtsam/inference/VariableIndex.h>
namespace gtsam { namespace gtsam {
@ -43,7 +43,7 @@ namespace gtsam {
// typedef MyJunctionTree JunctionTreeType; ///< Type of Junction tree (e.g. GaussianJunctionTree) // typedef MyJunctionTree JunctionTreeType; ///< Type of Junction tree (e.g. GaussianJunctionTree)
// static pair<shared_ptr<ConditionalType>, shared_ptr<FactorType> // static pair<shared_ptr<ConditionalType>, shared_ptr<FactorType>
// DefaultEliminate( // DefaultEliminate(
// const MyFactorGraph& factors, const OrderingUnordered& keys); ///< The default dense elimination function // const MyFactorGraph& factors, const Ordering& keys); ///< The default dense elimination function
}; };
@ -85,13 +85,13 @@ namespace gtsam {
typedef std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<_FactorType> > EliminationResult; typedef std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<_FactorType> > EliminationResult;
/// The function type that does a single dense elimination step on a subgraph. /// The function type that does a single dense elimination step on a subgraph.
typedef boost::function<EliminationResult(const FactorGraphType&, const OrderingUnordered&)> Eliminate; typedef boost::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
/// Typedef for an optional ordering as an argument to elimination functions /// Typedef for an optional ordering as an argument to elimination functions
typedef boost::optional<const OrderingUnordered&> OptionalOrdering; typedef boost::optional<const Ordering&> OptionalOrdering;
/// Typedef for an optional variable index as an argument to elimination functions /// Typedef for an optional variable index as an argument to elimination functions
typedef boost::optional<const VariableIndexUnordered&> OptionalVariableIndex; typedef boost::optional<const VariableIndex&> OptionalVariableIndex;
/** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not /** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not
* provided, the ordering provided by COLAMD will be used. * provided, the ordering provided by COLAMD will be used.
@ -149,7 +149,7 @@ namespace gtsam {
* B = X\backslash A \f$. */ * B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> > std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialSequential( eliminatePartialSequential(
const OrderingUnordered& ordering, const Ordering& ordering,
const Eliminate& function = EliminationTraits::DefaultEliminate, const Eliminate& function = EliminationTraits::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const; OptionalVariableIndex variableIndex = boost::none) const;
@ -169,7 +169,7 @@ namespace gtsam {
* factor graph, and \f$ B = X\backslash A \f$. */ * factor graph, and \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialMultifrontal( eliminatePartialMultifrontal(
const OrderingUnordered& ordering, const Ordering& ordering,
const Eliminate& function = EliminationTraits::DefaultEliminate, const Eliminate& function = EliminationTraits::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const; OptionalVariableIndex variableIndex = boost::none) const;
@ -195,7 +195,7 @@ namespace gtsam {
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
* provided one will be computed. */ * provided one will be computed. */
boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet( boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
boost::variant<const OrderingUnordered&, const std::vector<Key>&> variables, boost::variant<const Ordering&, const std::vector<Key>&> variables,
OptionalOrdering marginalizedVariableOrdering = boost::none, OptionalOrdering marginalizedVariableOrdering = boost::none,
const Eliminate& function = EliminationTraits::DefaultEliminate, const Eliminate& function = EliminationTraits::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const; OptionalVariableIndex variableIndex = boost::none) const;

38
gtsam/inference/EliminationTreeUnordered-inst.h → gtsam/inference/EliminationTree-inst.h
View File

@ -24,17 +24,17 @@
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h> #include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/inference/EliminationTreeUnordered.h> #include <gtsam/inference/EliminationTree.h>
#include <gtsam/inference/VariableIndexUnordered.h> #include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/OrderingUnordered.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/inference/inference-inst.h> #include <gtsam/inference/inference-inst.h>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESNET, class GRAPH> template<class BAYESNET, class GRAPH>
typename EliminationTreeUnordered<BAYESNET,GRAPH>::sharedFactor typename EliminationTree<BAYESNET,GRAPH>::sharedFactor
EliminationTreeUnordered<BAYESNET,GRAPH>::Node::eliminate( EliminationTree<BAYESNET,GRAPH>::Node::eliminate(
const boost::shared_ptr<BayesNetType>& output, const boost::shared_ptr<BayesNetType>& output,
const Eliminate& function, const std::vector<sharedFactor>& childrenResults) const const Eliminate& function, const std::vector<sharedFactor>& childrenResults) const
{ {
@ -51,7 +51,7 @@ namespace gtsam {
// Do dense elimination step // Do dense elimination step
std::vector<Key> keyAsVector(1); keyAsVector[0] = key; std::vector<Key> keyAsVector(1); keyAsVector[0] = key;
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult = std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
function(gatheredFactors, OrderingUnordered(keyAsVector)); function(gatheredFactors, Ordering(keyAsVector));
// Add conditional to BayesNet // Add conditional to BayesNet
output->push_back(eliminationResult.first); output->push_back(eliminationResult.first);
@ -62,7 +62,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESNET, class GRAPH> template<class BAYESNET, class GRAPH>
void EliminationTreeUnordered<BAYESNET,GRAPH>::Node::print( void EliminationTree<BAYESNET,GRAPH>::Node::print(
const std::string& str, const KeyFormatter& keyFormatter) const const std::string& str, const KeyFormatter& keyFormatter) const
{ {
std::cout << str << "(" << keyFormatter(key) << ")\n"; std::cout << str << "(" << keyFormatter(key) << ")\n";
@ -77,8 +77,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESNET, class GRAPH> template<class BAYESNET, class GRAPH>
EliminationTreeUnordered<BAYESNET,GRAPH>::EliminationTreeUnordered(const FactorGraphType& graph, EliminationTree<BAYESNET,GRAPH>::EliminationTree(const FactorGraphType& graph,
const VariableIndexUnordered& structure, const OrderingUnordered& order) const VariableIndex& structure, const Ordering& order)
{ {
gttic(EliminationTree_Contructor); gttic(EliminationTree_Contructor);
@ -100,7 +100,7 @@ namespace gtsam {
for (size_t j = 0; j < n; j++) for (size_t j = 0; j < n; j++)
{ {
// Retrieve the factors involving this variable and create the current node // Retrieve the factors involving this variable and create the current node
const VariableIndexUnordered::Factors& factors = structure[order[j]]; const VariableIndex::Factors& factors = structure[order[j]];
nodes[j] = boost::make_shared<Node>(); nodes[j] = boost::make_shared<Node>();
nodes[j]->key = order[j]; nodes[j]->key = order[j];
@ -157,20 +157,20 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESNET, class GRAPH> template<class BAYESNET, class GRAPH>
EliminationTreeUnordered<BAYESNET,GRAPH>::EliminationTreeUnordered( EliminationTree<BAYESNET,GRAPH>::EliminationTree(
const FactorGraphType& factorGraph, const OrderingUnordered& order) const FactorGraphType& factorGraph, const Ordering& order)
{ {
gttic(ET_Create2); gttic(ET_Create2);
// Build variable index first // Build variable index first
const VariableIndexUnordered variableIndex(factorGraph); const VariableIndex variableIndex(factorGraph);
This temp(factorGraph, variableIndex, order); This temp(factorGraph, variableIndex, order);
this->swap(temp); // Swap in the tree, and temp will be deleted this->swap(temp); // Swap in the tree, and temp will be deleted
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESNET, class GRAPH> template<class BAYESNET, class GRAPH>
EliminationTreeUnordered<BAYESNET,GRAPH>& EliminationTree<BAYESNET,GRAPH>&
EliminationTreeUnordered<BAYESNET,GRAPH>::operator=(const EliminationTreeUnordered<BAYESNET,GRAPH>& other) EliminationTree<BAYESNET,GRAPH>::operator=(const EliminationTree<BAYESNET,GRAPH>& other)
{ {
// Start by duplicating the tree. // Start by duplicating the tree.
roots_ = treeTraversal::CloneForest(other); roots_ = treeTraversal::CloneForest(other);
@ -185,7 +185,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESNET, class GRAPH> template<class BAYESNET, class GRAPH>
std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<GRAPH> > std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<GRAPH> >
EliminationTreeUnordered<BAYESNET,GRAPH>::eliminate(Eliminate function) const EliminationTree<BAYESNET,GRAPH>::eliminate(Eliminate function) const
{ {
gttic(EliminationTree_eliminate); gttic(EliminationTree_eliminate);
// Allocate result // Allocate result
@ -205,14 +205,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESNET, class GRAPH> template<class BAYESNET, class GRAPH>
void EliminationTreeUnordered<BAYESNET,GRAPH>::print(const std::string& name, const KeyFormatter& formatter) const void EliminationTree<BAYESNET,GRAPH>::print(const std::string& name, const KeyFormatter& formatter) const
{ {
treeTraversal::PrintForest(*this, name, formatter); treeTraversal::PrintForest(*this, name, formatter);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESNET, class GRAPH> template<class BAYESNET, class GRAPH>
bool EliminationTreeUnordered<BAYESNET,GRAPH>::equals(const This& expected, double tol) const bool EliminationTree<BAYESNET,GRAPH>::equals(const This& expected, double tol) const
{ {
// Depth-first-traversal stacks // Depth-first-traversal stacks
std::stack<sharedNode, std::vector<sharedNode> > stack1, stack2; std::stack<sharedNode, std::vector<sharedNode> > stack1, stack2;
@ -281,7 +281,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESNET, class GRAPH> template<class BAYESNET, class GRAPH>
void EliminationTreeUnordered<BAYESNET,GRAPH>::swap(This& other) { void EliminationTree<BAYESNET,GRAPH>::swap(This& other) {
roots_.swap(other.roots_); roots_.swap(other.roots_);
remainingFactors_.swap(other.remainingFactors_); remainingFactors_.swap(other.remainingFactors_);
} }

354
gtsam/inference/EliminationTree.h
View File

@ -1,187 +1,167 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file EliminationTree.h * @file EliminationTree.h
* @author Frank Dellaert * @author Frank Dellaert
* @date Oct 13, 2010 * @author Richard Roberts
*/ * @date Oct 13, 2010
#pragma once */
#pragma once
#include <utility>
#include <utility>
#include <gtsam/base/FastSet.h> #include <boost/shared_ptr.hpp>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/VariableIndex.h> #include <gtsam/base/Testable.h>
#include <gtsam/inference/FactorGraph.h>
class EliminationTreeTester; // for unit tests, see testEliminationTree
class EliminationTreeTester; // for unit tests, see testEliminationTree
namespace gtsam { template<class CONDITIONAL> class BayesNet; } namespace gtsam {
namespace gtsam { class VariableIndex;
class Ordering;
/**
* An elimination tree is a data structure used intermediately during /**
* elimination. In future versions it will be used to save work between * An elimination tree is a data structure used intermediately during
* multiple eliminations. * elimination. In future versions it will be used to save work between
* * multiple eliminations.
* When a variable is eliminated, a new factor is created by combining that *
* variable's neighboring factors. The new combined factor involves the combined * When a variable is eliminated, a new factor is created by combining that
* factors' involved variables. When the lowest-ordered one of those variables * variable's neighboring factors. The new combined factor involves the combined
* is eliminated, it consumes that combined factor. In the elimination tree, * factors' involved variables. When the lowest-ordered one of those variables
* that lowest-ordered variable is the parent of the variable that was eliminated to * is eliminated, it consumes that combined factor. In the elimination tree,
* produce the combined factor. This yields a tree in general, and not a chain * that lowest-ordered variable is the parent of the variable that was eliminated to
* because of the implicit sparse structure of the resulting Bayes net. * produce the combined factor. This yields a tree in general, and not a chain
* * because of the implicit sparse structure of the resulting Bayes net.
* This structure is examined even more closely in a JunctionTree, which *
* additionally identifies cliques in the chordal Bayes net. * This structure is examined even more closely in a JunctionTree, which
* \nosubgrouping * additionally identifies cliques in the chordal Bayes net.
*/ * \nosubgrouping
template<class FACTOR> */
class EliminationTree { template<class BAYESNET, class GRAPH>
class EliminationTree
public: {
protected:
typedef EliminationTree<FACTOR> This; ///< This class typedef EliminationTree<BAYESNET, GRAPH> This; ///< This class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
typedef typename boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
typedef gtsam::BayesNet<typename FACTOR::ConditionalType> BayesNet; ///< The BayesNet corresponding to FACTOR public:
typedef FACTOR Factor; typedef GRAPH FactorGraphType; ///< The factor graph type
typedef typename FACTOR::KeyType KeyType; typedef typename GRAPH::FactorType FactorType; ///< The type of factors
typedef typename boost::shared_ptr<FactorType> sharedFactor; ///< Shared pointer to a factor
/** Typedef for an eliminate subroutine */ typedef BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR
typedef typename FactorGraph<FACTOR>::Eliminate Eliminate; typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals
typedef typename boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
private: typedef typename GRAPH::Eliminate Eliminate;
/** concept check */ struct Node {
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) typedef std::vector<sharedFactor> Factors;
typedef std::vector<boost::shared_ptr<Node> > Children;
typedef FastList<sharedFactor> Factors;
typedef FastList<shared_ptr> SubTrees; Key key; ///< key associated with root
typedef std::vector<typename FACTOR::ConditionalType::shared_ptr> Conditionals; Factors factors; ///< factors associated with root
Children children; ///< sub-trees
Index key_; ///< index associated with root // FIXME: doesn't this require that "Index" is the type of keys in the generic factor?
Factors factors_; ///< factors associated with root sharedFactor eliminate(const boost::shared_ptr<BayesNetType>& output,
SubTrees subTrees_; ///< sub-trees const Eliminate& function, const std::vector<sharedFactor>& childrenFactors) const;
public: void print(const std::string& str, const KeyFormatter& keyFormatter) const;
};
/// @name Standard Constructors
/// @{ typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
/** protected:
* Named constructor to build the elimination tree of a factor graph using /** concept check */
* pre-computed column structure. GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
* @param factorGraph The factor graph for which to build the elimination tree
* @param structure The set of factors involving each variable. If this is not std::vector<sharedNode> roots_;
* precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&) std::vector<sharedFactor> remainingFactors_;
* named constructor instead.
* @return The elimination tree /// @name Standard Constructors
*/ /// @{
template<class DERIVEDFACTOR>
static shared_ptr Create(const FactorGraph<DERIVEDFACTOR>& factorGraph, const VariableIndex& structure); /**
* Build the elimination tree of a factor graph using pre-computed column structure.
/** Named constructor to build the elimination tree of a factor graph. Note * @param factorGraph The factor graph for which to build the elimination tree
* that this has to compute the column structure as a VariableIndex, so if you * @param structure The set of factors involving each variable. If this is not
* already have this precomputed, use the Create(const FactorGraph<DERIVEDFACTOR>&, const VariableIndex&) * precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
* named constructor instead. * named constructor instead.
* @param factorGraph The factor graph for which to build the elimination tree * @return The elimination tree
*/ */
template<class DERIVEDFACTOR> EliminationTree(const FactorGraphType& factorGraph,
static shared_ptr Create(const FactorGraph<DERIVEDFACTOR>& factorGraph); const VariableIndex& structure, const Ordering& order);
/// @} /** Build the elimination tree of a factor graph. Note that this has to compute the column
/// @name Standard Interface * structure as a VariableIndex, so if you already have this precomputed, use the other
/// @{ * constructor instead.
* @param factorGraph The factor graph for which to build the elimination tree
/** Eliminate the factors to a Bayes Net */
* @param function The function to use to eliminate, see the namespace functions EliminationTree(const FactorGraphType& factorGraph, const Ordering& order);
* in GaussianFactorGraph.h
* @return The BayesNet resulting from elimination /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
*/ * copied, factors are not cloned. */
typename BayesNet::shared_ptr eliminate(Eliminate function) const; EliminationTree(const This& other) { *this = other; }
/** Eliminate the factors to a Bayes Net and return the remaining factor /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors
* @param function The function to use to eliminate, see the namespace functions * are copied, factors are not cloned. */
* in GaussianFactorGraph.h This& operator=(const This& other);
* @return The BayesNet resulting from elimination and the remaining factor
*/ /// @}
typename BayesNet::shared_ptr eliminatePartial(Eliminate function, size_t nrToEliminate) const;
public:
/// @} /// @name Standard Interface
/// @name Testable /// @{
/// @{
/** Eliminate the factors to a Bayes net and remaining factor graph
/** Print the tree to cout */ * @param function The function to use to eliminate, see the namespace functions
void print(const std::string& name = "EliminationTree: ", * in GaussianFactorGraph.h
const IndexFormatter& formatter = DefaultIndexFormatter) const; * @return The Bayes net and factor graph resulting from elimination
*/
/** Test whether the tree is equal to another */ std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
bool equals(const EliminationTree& other, double tol = 1e-9) const; eliminate(Eliminate function) const;
/// @} /// @}
/// @name Testable
private: /// @{
/** default constructor, private, as you should use Create below */ /** Print the tree to cout */
EliminationTree(Index key = 0) : key_(key) {} void print(const std::string& name = "EliminationTree: ",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
/**
* Static internal function to build a vector of parent pointers using the protected:
* algorithm of Gilbert et al., 2001, BIT. /** Test whether the tree is equal to another */
*/ bool equals(const This& other, double tol = 1e-9) const;
static std::vector<Index> ComputeParents(const VariableIndex& structure);
/// @}
/** add a factor, for Create use only */
void add(const sharedFactor& factor) { factors_.push_back(factor); } public:
/// @name Advanced Interface
/** add a subtree, for Create use only */ /// @{
void add(const shared_ptr& child) { subTrees_.push_back(child); }
/** Return the set of roots (one for a tree, multiple for a forest) */
/** const std::vector<sharedNode>& roots() const { return roots_; }
* Recursive routine that eliminates the factors arranged in an elimination tree
* @param Conditionals is a vector of shared pointers that will be modified in place /** Return the remaining factors that are not pulled into elimination */
*/ const std::vector<sharedFactor>& remainingFactors() const { return remainingFactors_; }
sharedFactor eliminate_(Eliminate function, Conditionals& conditionals) const;
/** Swap the data of this tree with another one, this operation is very fast. */
/// Allow access to constructor and add methods for testing purposes void swap(This& other);
friend class ::EliminationTreeTester;
protected:
}; /// Protected default constructor
EliminationTree() {}
/** private:
* An exception thrown when attempting to eliminate a disconnected factor /// Allow access to constructor and add methods for testing purposes
* graph, which is not currently possible in gtsam. If you need to work with friend class ::EliminationTreeTester;
* disconnected graphs, a workaround is to create zero-information factors to };
* bridge the disconnects. To do this, create any factor type (e.g.
* BetweenFactor or RangeFactor) with the noise model }
* <tt>sharedPrecision(dim, 0.0)</tt>, where \c dim is the appropriate
* dimensionality for that factor.
*/
struct DisconnectedGraphException : public std::exception {
DisconnectedGraphException() {}
virtual ~DisconnectedGraphException() throw() {}
/// Returns the string "Attempting to eliminate a disconnected graph - this is not currently possible in gtsam."
virtual const char* what() const throw() {
return
"Attempting to eliminate a disconnected graph - this is not currently possible in\n"
"GTSAM. You may add \"empty\" BetweenFactor's to join disconnected graphs, these\n"
"will affect the symbolic structure and solving complexity of the graph but not\n"
"the solution. To do this, create BetweenFactor's with zero-precision noise\n"
"models, i.e. noiseModel::Isotropic::Precision(n, 0.0);\n"; }
};
}
#include <gtsam/inference/EliminationTree-inl.h>

40
gtsam/inference/EliminationTree-inl.h → gtsam/inference/EliminationTreeOrdered-inl.h
View File

@ -18,10 +18,10 @@
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/base/FastSet.h> #include <gtsam/base/FastSet.h>
#include <gtsam/inference/EliminationTree.h> #include <gtsam/inference/EliminationTreeOrdered.h>
#include <gtsam/inference/VariableSlots.h> #include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/IndexFactor.h> #include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/IndexConditional.h> #include <gtsam/inference/IndexConditionalOrdered.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/static_assert.hpp> #include <boost/static_assert.hpp>
@ -34,7 +34,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminate_( typename EliminationTreeOrdered<FACTOR>::sharedFactor EliminationTreeOrdered<FACTOR>::eliminate_(
Eliminate function, Conditionals& conditionals) const { Eliminate function, Conditionals& conditionals) const {
static const bool debug = false; static const bool debug = false;
@ -43,7 +43,7 @@ typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminat
if(this->key_ < conditionals.size()) { // If it is requested to eliminate the current variable if(this->key_ < conditionals.size()) { // If it is requested to eliminate the current variable
// Create the list of factors to be eliminated, initially empty, and reserve space // Create the list of factors to be eliminated, initially empty, and reserve space
FactorGraph<FACTOR> factors; FactorGraphOrdered<FACTOR> factors;
factors.reserve(this->factors_.size() + this->subTrees_.size()); factors.reserve(this->factors_.size() + this->subTrees_.size());
// Add all factors associated with the current node // Add all factors associated with the current node
@ -55,7 +55,7 @@ typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminat
// TODO: wait for completion of all threads // TODO: wait for completion of all threads
// Combine all factors (from this node and from subtrees) into a joint factor // Combine all factors (from this node and from subtrees) into a joint factor
typename FactorGraph<FACTOR>::EliminationResult typename FactorGraphOrdered<FACTOR>::EliminationResult
eliminated(function(factors, 1)); eliminated(function(factors, 1));
conditionals[this->key_] = eliminated.first; conditionals[this->key_] = eliminated.first;
@ -75,7 +75,7 @@ typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminat
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
std::vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) { std::vector<Index> EliminationTreeOrdered<FACTOR>::ComputeParents(const VariableIndexOrdered& structure) {
// Number of factors and variables // Number of factors and variables
const size_t m = structure.nFactors(); const size_t m = structure.nFactors();
@ -109,9 +109,9 @@ std::vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex&
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create( typename EliminationTreeOrdered<FACTOR>::shared_ptr EliminationTreeOrdered<FACTOR>::Create(
const FactorGraph<DERIVEDFACTOR>& factorGraph, const FactorGraphOrdered<DERIVEDFACTOR>& factorGraph,
const VariableIndex& structure) { const VariableIndexOrdered& structure) {
static const bool debug = false; static const bool debug = false;
gttic(ET_Create1); gttic(ET_Create1);
@ -131,7 +131,7 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
std::vector<shared_ptr> trees(n); std::vector<shared_ptr> trees(n);
for (Index k = 1; k <= n; k++) { for (Index k = 1; k <= n; k++) {
Index j = n - k; // Start at the last variable and loop down to 0 Index j = n - k; // Start at the last variable and loop down to 0
trees[j].reset(new EliminationTree(j)); // Create a new node on this variable trees[j].reset(new EliminationTreeOrdered(j)); // Create a new node on this variable
if (parents[j] != none) // If this node has a parent, add it to the parent's children if (parents[j] != none) // If this node has a parent, add it to the parent's children
trees[parents[j]]->add(trees[j]); trees[parents[j]]->add(trees[j]);
else if(!structure[j].empty() && j != n - 1) // If a node other than the last has no parents, this is a forest else if(!structure[j].empty() && j != n - 1) // If a node other than the last has no parents, this is a forest
@ -161,12 +161,12 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
typename EliminationTree<FACTOR>::shared_ptr typename EliminationTreeOrdered<FACTOR>::shared_ptr
EliminationTree<FACTOR>::Create(const FactorGraph<DERIVEDFACTOR>& factorGraph) { EliminationTreeOrdered<FACTOR>::Create(const FactorGraphOrdered<DERIVEDFACTOR>& factorGraph) {
gttic(ET_Create2); gttic(ET_Create2);
// Build variable index // Build variable index
const VariableIndex variableIndex(factorGraph); const VariableIndexOrdered variableIndex(factorGraph);
// Build elimination tree // Build elimination tree
return Create(factorGraph, variableIndex); return Create(factorGraph, variableIndex);
@ -174,7 +174,7 @@ EliminationTree<FACTOR>::Create(const FactorGraph<DERIVEDFACTOR>& factorGraph) {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTORGRAPH> template<class FACTORGRAPH>
void EliminationTree<FACTORGRAPH>::print(const std::string& name, void EliminationTreeOrdered<FACTORGRAPH>::print(const std::string& name,
const IndexFormatter& formatter) const { const IndexFormatter& formatter) const {
std::cout << name << " (" << formatter(key_) << ")" << std::endl; std::cout << name << " (" << formatter(key_) << ")" << std::endl;
BOOST_FOREACH(const sharedFactor& factor, factors_) { BOOST_FOREACH(const sharedFactor& factor, factors_) {
@ -185,7 +185,7 @@ void EliminationTree<FACTORGRAPH>::print(const std::string& name,
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTORGRAPH> template<class FACTORGRAPH>
bool EliminationTree<FACTORGRAPH>::equals(const EliminationTree<FACTORGRAPH>& expected, double tol) const { bool EliminationTreeOrdered<FACTORGRAPH>::equals(const EliminationTreeOrdered<FACTORGRAPH>& expected, double tol) const {
if(this->key_ == expected.key_ && this->factors_ == expected.factors_ if(this->key_ == expected.key_ && this->factors_ == expected.factors_
&& this->subTrees_.size() == expected.subTrees_.size()) { && this->subTrees_.size() == expected.subTrees_.size()) {
typename SubTrees::const_iterator this_subtree = this->subTrees_.begin(); typename SubTrees::const_iterator this_subtree = this->subTrees_.begin();
@ -200,8 +200,8 @@ bool EliminationTree<FACTORGRAPH>::equals(const EliminationTree<FACTORGRAPH>& ex
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
typename EliminationTree<FACTOR>::BayesNet::shared_ptr typename EliminationTreeOrdered<FACTOR>::BayesNet::shared_ptr
EliminationTree<FACTOR>::eliminatePartial(typename EliminationTree<FACTOR>::Eliminate function, size_t nrToEliminate) const { EliminationTreeOrdered<FACTOR>::eliminatePartial(typename EliminationTreeOrdered<FACTOR>::Eliminate function, size_t nrToEliminate) const {
// call recursive routine // call recursive routine
gttic(ET_recursive_eliminate); gttic(ET_recursive_eliminate);
@ -225,8 +225,8 @@ typename EliminationTree<FACTOR>::BayesNet::shared_ptr
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
typename EliminationTree<FACTOR>::BayesNet::shared_ptr typename EliminationTreeOrdered<FACTOR>::BayesNet::shared_ptr
EliminationTree<FACTOR>::eliminate(Eliminate function) const { EliminationTreeOrdered<FACTOR>::eliminate(Eliminate function) const {
size_t nrConditionals = this->key_ + 1; // root key has highest index size_t nrConditionals = this->key_ + 1; // root key has highest index
return eliminatePartial(function, nrConditionals); return eliminatePartial(function, nrConditionals);
} }

187
gtsam/inference/EliminationTreeOrdered.h Normal file
View File

@ -0,0 +1,187 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file EliminationTree.h
* @author Frank Dellaert
* @date Oct 13, 2010
*/
#pragma once
#include <utility>
#include <gtsam/base/FastSet.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/VariableIndexOrdered.h>
#include <gtsam/inference/FactorGraphOrdered.h>
class EliminationTreeOrderedTester; // for unit tests, see testEliminationTree
namespace gtsam { template<class CONDITIONAL> class BayesNetOrdered; }
namespace gtsam {
/**
* An elimination tree is a data structure used intermediately during
* elimination. In future versions it will be used to save work between
* multiple eliminations.
*
* When a variable is eliminated, a new factor is created by combining that
* variable's neighboring factors. The new combined factor involves the combined
* factors' involved variables. When the lowest-ordered one of those variables
* is eliminated, it consumes that combined factor. In the elimination tree,
* that lowest-ordered variable is the parent of the variable that was eliminated to
* produce the combined factor. This yields a tree in general, and not a chain
* because of the implicit sparse structure of the resulting Bayes net.
*
* This structure is examined even more closely in a JunctionTree, which
* additionally identifies cliques in the chordal Bayes net.
* \nosubgrouping
*/
template<class FACTOR>
class EliminationTreeOrdered {
public:
typedef EliminationTreeOrdered<FACTOR> This; ///< This class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
typedef typename boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
typedef gtsam::BayesNetOrdered<typename FACTOR::ConditionalType> BayesNet; ///< The BayesNet corresponding to FACTOR
typedef FACTOR Factor;
typedef typename FACTOR::KeyType KeyType;
/** Typedef for an eliminate subroutine */
typedef typename FactorGraphOrdered<FACTOR>::Eliminate Eliminate;
private:
/** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
typedef FastList<sharedFactor> Factors;
typedef FastList<shared_ptr> SubTrees;
typedef std::vector<typename FACTOR::ConditionalType::shared_ptr> Conditionals;
Index key_; ///< index associated with root // FIXME: doesn't this require that "Index" is the type of keys in the generic factor?
Factors factors_; ///< factors associated with root
SubTrees subTrees_; ///< sub-trees
public:
/// @name Standard Constructors
/// @{
/**
* Named constructor to build the elimination tree of a factor graph using
* pre-computed column structure.
* @param factorGraph The factor graph for which to build the elimination tree
* @param structure The set of factors involving each variable. If this is not
* precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
* named constructor instead.
* @return The elimination tree
*/
template<class DERIVEDFACTOR>
static shared_ptr Create(const FactorGraphOrdered<DERIVEDFACTOR>& factorGraph, const VariableIndexOrdered& structure);
/** Named constructor to build the elimination tree of a factor graph. Note
* that this has to compute the column structure as a VariableIndex, so if you
* already have this precomputed, use the Create(const FactorGraph<DERIVEDFACTOR>&, const VariableIndex&)
* named constructor instead.
* @param factorGraph The factor graph for which to build the elimination tree
*/
template<class DERIVEDFACTOR>
static shared_ptr Create(const FactorGraphOrdered<DERIVEDFACTOR>& factorGraph);
/// @}
/// @name Standard Interface
/// @{
/** Eliminate the factors to a Bayes Net
* @param function The function to use to eliminate, see the namespace functions
* in GaussianFactorGraph.h
* @return The BayesNet resulting from elimination
*/
typename BayesNet::shared_ptr eliminate(Eliminate function) const;
/** Eliminate the factors to a Bayes Net and return the remaining factor
* @param function The function to use to eliminate, see the namespace functions
* in GaussianFactorGraph.h
* @return The BayesNet resulting from elimination and the remaining factor
*/
typename BayesNet::shared_ptr eliminatePartial(Eliminate function, size_t nrToEliminate) const;
/// @}
/// @name Testable
/// @{
/** Print the tree to cout */
void print(const std::string& name = "EliminationTree: ",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** Test whether the tree is equal to another */
bool equals(const EliminationTreeOrdered& other, double tol = 1e-9) const;
/// @}
private:
/** default constructor, private, as you should use Create below */
EliminationTreeOrdered(Index key = 0) : key_(key) {}
/**
* Static internal function to build a vector of parent pointers using the
* algorithm of Gilbert et al., 2001, BIT.
*/
static std::vector<Index> ComputeParents(const VariableIndexOrdered& structure);
/** add a factor, for Create use only */
void add(const sharedFactor& factor) { factors_.push_back(factor); }
/** add a subtree, for Create use only */
void add(const shared_ptr& child) { subTrees_.push_back(child); }
/**
* Recursive routine that eliminates the factors arranged in an elimination tree
* @param Conditionals is a vector of shared pointers that will be modified in place
*/
sharedFactor eliminate_(Eliminate function, Conditionals& conditionals) const;
/// Allow access to constructor and add methods for testing purposes
friend class ::EliminationTreeOrderedTester;
};
/**
* An exception thrown when attempting to eliminate a disconnected factor
* graph, which is not currently possible in gtsam. If you need to work with
* disconnected graphs, a workaround is to create zero-information factors to
* bridge the disconnects. To do this, create any factor type (e.g.
* BetweenFactor or RangeFactor) with the noise model
* <tt>sharedPrecision(dim, 0.0)</tt>, where \c dim is the appropriate
* dimensionality for that factor.
*/
struct DisconnectedGraphException : public std::exception {
DisconnectedGraphException() {}
virtual ~DisconnectedGraphException() throw() {}
/// Returns the string "Attempting to eliminate a disconnected graph - this is not currently possible in gtsam."
virtual const char* what() const throw() {
return
"Attempting to eliminate a disconnected graph - this is not currently possible in\n"
"GTSAM. You may add \"empty\" BetweenFactor's to join disconnected graphs, these\n"
"will affect the symbolic structure and solving complexity of the graph but not\n"
"the solution. To do this, create BetweenFactor's with zero-precision noise\n"
"models, i.e. noiseModel::Isotropic::Precision(n, 0.0);\n"; }
};
}
#include <gtsam/inference/EliminationTreeOrdered-inl.h>

167
gtsam/inference/EliminationTreeUnordered.h
View File

@ -1,167 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file EliminationTree.h
* @author Frank Dellaert
* @author Richard Roberts
* @date Oct 13, 2010
*/
#pragma once
#include <utility>
#include <boost/shared_ptr.hpp>
#include <gtsam/base/Testable.h>
class EliminationTreeUnorderedTester; // for unit tests, see testEliminationTree
namespace gtsam {
class VariableIndexUnordered;
class OrderingUnordered;
/**
* An elimination tree is a data structure used intermediately during
* elimination. In future versions it will be used to save work between
* multiple eliminations.
*
* When a variable is eliminated, a new factor is created by combining that
* variable's neighboring factors. The new combined factor involves the combined
* factors' involved variables. When the lowest-ordered one of those variables
* is eliminated, it consumes that combined factor. In the elimination tree,
* that lowest-ordered variable is the parent of the variable that was eliminated to
* produce the combined factor. This yields a tree in general, and not a chain
* because of the implicit sparse structure of the resulting Bayes net.
*
* This structure is examined even more closely in a JunctionTree, which
* additionally identifies cliques in the chordal Bayes net.
* \nosubgrouping
*/
template<class BAYESNET, class GRAPH>
class EliminationTreeUnordered
{
protected:
typedef EliminationTreeUnordered<BAYESNET, GRAPH> This; ///< This class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
public:
typedef GRAPH FactorGraphType; ///< The factor graph type
typedef typename GRAPH::FactorType FactorType; ///< The type of factors
typedef typename boost::shared_ptr<FactorType> sharedFactor; ///< Shared pointer to a factor
typedef BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR
typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals
typedef typename boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
typedef typename GRAPH::Eliminate Eliminate;
struct Node {
typedef std::vector<sharedFactor> Factors;
typedef std::vector<boost::shared_ptr<Node> > Children;
Key key; ///< key associated with root
Factors factors; ///< factors associated with root
Children children; ///< sub-trees
sharedFactor eliminate(const boost::shared_ptr<BayesNetType>& output,
const Eliminate& function, const std::vector<sharedFactor>& childrenFactors) const;
void print(const std::string& str, const KeyFormatter& keyFormatter) const;
};
typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
protected:
/** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
std::vector<sharedNode> roots_;
std::vector<sharedFactor> remainingFactors_;
/// @name Standard Constructors
/// @{
/**
* Build the elimination tree of a factor graph using pre-computed column structure.
* @param factorGraph The factor graph for which to build the elimination tree
* @param structure The set of factors involving each variable. If this is not
* precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
* named constructor instead.
* @return The elimination tree
*/
EliminationTreeUnordered(const FactorGraphType& factorGraph,
const VariableIndexUnordered& structure, const OrderingUnordered& order);
/** Build the elimination tree of a factor graph. Note that this has to compute the column
* structure as a VariableIndex, so if you already have this precomputed, use the other
* constructor instead.
* @param factorGraph The factor graph for which to build the elimination tree
*/
EliminationTreeUnordered(const FactorGraphType& factorGraph, const OrderingUnordered& order);
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
* copied, factors are not cloned. */
EliminationTreeUnordered(const This& other) { *this = other; }
/** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors
* are copied, factors are not cloned. */
This& operator=(const This& other);
/// @}
public:
/// @name Standard Interface
/// @{
/** Eliminate the factors to a Bayes net and remaining factor graph
* @param function The function to use to eliminate, see the namespace functions
* in GaussianFactorGraph.h
* @return The Bayes net and factor graph resulting from elimination
*/
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
eliminate(Eliminate function) const;
/// @}
/// @name Testable
/// @{
/** Print the tree to cout */
void print(const std::string& name = "EliminationTree: ",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
protected:
/** Test whether the tree is equal to another */
bool equals(const This& other, double tol = 1e-9) const;
/// @}
public:
/// @name Advanced Interface
/// @{
/** Return the set of roots (one for a tree, multiple for a forest) */
const std::vector<sharedNode>& roots() const { return roots_; }
/** Return the remaining factors that are not pulled into elimination */
const std::vector<sharedFactor>& remainingFactors() const { return remainingFactors_; }
/** Swap the data of this tree with another one, this operation is very fast. */
void swap(This& other);
protected:
/// Protected default constructor
EliminationTreeUnordered() {}
private:
/// Allow access to constructor and add methods for testing purposes
friend class ::EliminationTreeUnorderedTester;
};
}

8
gtsam/inference/FactorUnordered.cpp → gtsam/inference/Factor.cpp
View File

@ -22,25 +22,25 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <iostream> #include <iostream>
#include <gtsam/inference/FactorUnordered.h> #include <gtsam/inference/Factor.h>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void FactorUnordered::print(const std::string& s, const KeyFormatter& formatter) const void Factor::print(const std::string& s, const KeyFormatter& formatter) const
{ {
return this->printKeys(s, formatter); return this->printKeys(s, formatter);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void FactorUnordered::printKeys(const std::string& s, const KeyFormatter& formatter) const { void Factor::printKeys(const std::string& s, const KeyFormatter& formatter) const {
std::cout << s << " "; std::cout << s << " ";
BOOST_FOREACH(Key key, keys_) std::cout << " " << formatter(key); BOOST_FOREACH(Key key, keys_) std::cout << " " << formatter(key);
std::cout << std::endl; std::cout << std::endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool FactorUnordered::equals(const This& other, double tol) const { bool Factor::equals(const This& other, double tol) const {
return keys_ == other.keys_; return keys_ == other.keys_;
} }

271
gtsam/inference/Factor.h
View File

@ -21,212 +21,151 @@
#pragma once #pragma once
#include <set>
#include <vector> #include <vector>
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <boost/foreach.hpp>
#include <boost/function/function1.hpp> #include <gtsam/base/types.h>
#include <boost/lexical_cast.hpp> #include <gtsam/inference/Key.h>
#include <gtsam/global_includes.h>
#include <gtsam/base/FastMap.h>
namespace gtsam { namespace gtsam {
template<class KEY> class Conditional;
/**
* This is the base class for all factor types. It is templated on a KEY type,
* which will be the type used to label variables. Key types currently in use
* in gtsam are Index with symbolic (IndexFactor, SymbolicFactorGraph) and
* Gaussian factors (GaussianFactor, JacobianFactor, HessianFactor, GaussianFactorGraph),
* and Key with nonlinear factors (NonlinearFactor, NonlinearFactorGraph).
* though currently only IndexFactor and IndexConditional derive from this
* class, using Index keys. This class does not store any data other than its
* keys. Derived classes store data such as matrices and probability tables.
*
* Note that derived classes *must* redefine the ConditionalType and shared_ptr
* typedefs to refer to the associated conditional and shared_ptr types of the
* derived class. See IndexFactor, JacobianFactor, etc. for examples.
*
* This class is \b not virtual for performance reasons - derived symbolic classes,
* IndexFactor and IndexConditional, need to be created and destroyed quickly
* during symbolic elimination. GaussianFactor and NonlinearFactor are virtual.
* \nosubgrouping
*/
template<typename KEY>
class Factor {
public:
typedef KEY KeyType; ///< The KEY template parameter
typedef Factor<KeyType> This; ///< This class
/** /**
* Typedef to the conditional type obtained by eliminating this factor, * This is the base class for all factor types. It is templated on a KEY type,
* derived classes must redefine this. * which will be the type used to label variables. Key types currently in use
* in gtsam are Index with symbolic (IndexFactor, SymbolicFactorGraph) and
* Gaussian factors (GaussianFactor, JacobianFactor, HessianFactor, GaussianFactorGraph),
* and Key with nonlinear factors (NonlinearFactor, NonlinearFactorGraph).
* though currently only IndexFactor and IndexConditional derive from this
* class, using Index keys. This class does not store any data other than its
* keys. Derived classes store data such as matrices and probability tables.
*
* Note that derived classes *must* redefine the ConditionalType and shared_ptr
* typedefs to refer to the associated conditional and shared_ptr types of the
* derived class. See IndexFactor, JacobianFactor, etc. for examples.
*
* This class is \b not virtual for performance reasons - derived symbolic classes,
* IndexFactor and IndexConditional, need to be created and destroyed quickly
* during symbolic elimination. GaussianFactor and NonlinearFactor are virtual.
* \nosubgrouping
*/ */
typedef Conditional<KeyType> ConditionalType; class GTSAM_EXPORT Factor
{
/// A shared_ptr to this class, derived classes must redefine this. private:
typedef boost::shared_ptr<Factor> shared_ptr; // These typedefs are private because they must be overridden in derived classes.
typedef Factor This; ///< This class
typedef boost::shared_ptr<Factor> shared_ptr; ///< A shared_ptr to this class.
/// Iterator over keys public:
typedef typename std::vector<KeyType>::iterator iterator; /// Iterator over keys
typedef std::vector<Key>::iterator iterator;
/// Const iterator over keys /// Const iterator over keys
typedef typename std::vector<KeyType>::const_iterator const_iterator; typedef std::vector<Key>::const_iterator const_iterator;
protected: protected:
/// The keys involved in this factor /// The keys involved in this factor
std::vector<KeyType> keys_; std::vector<Key> keys_;
public: /// @name Standard Constructors
/// @{
/// @name Standard Constructors /** Default constructor for I/O */
/// @{ Factor() {}
/** Copy constructor */ /** Construct factor from container of keys. This constructor is used internally from derived factor
Factor(const This& f); * constructors, either from a container of keys or from a boost::assign::list_of. */
template<typename CONTAINER>
Factor(const CONTAINER& keys) : keys_(keys.begin(), keys.end()) {}
/** Construct from conditional, calls ConditionalType::toFactor() */ /** Construct factor from iterator keys. This constructor may be used internally from derived
Factor(const ConditionalType& c); * factor constructors, although our code currently does not use this. */
template<typename ITERATOR>
Factor(ITERATOR first, ITERATOR last) : keys_(first, last) {}
/** Default constructor for I/O */ /** Construct factor from container of keys. This is called internally from derived factor static
Factor() {} * factor methods, as a workaround for not being able to call the protected constructors above. */
template<typename CONTAINER>
static Factor FromKeys(const CONTAINER& keys) {
return Factor(keys.begin(), keys.end()); }
/** Construct unary factor */ /** Construct factor from iterator keys. This is called internally from derived factor static
Factor(KeyType key) : keys_(1) { * factor methods, as a workaround for not being able to call the protected constructors above. */
keys_[0] = key; assertInvariants(); } template<typename ITERATOR>
static Factor FromIterators(ITERATOR first, ITERATOR last) {
return Factor(first, last); }
/** Construct binary factor */ /// @}
Factor(KeyType key1, KeyType key2) : keys_(2) {
keys_[0] = key1; keys_[1] = key2; assertInvariants(); }
/** Construct ternary factor */ public:
Factor(KeyType key1, KeyType key2, KeyType key3) : keys_(3) { /// @name Standard Interface
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); } /// @{
/** Construct 4-way factor */ /// First key
Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4) : keys_(4) { Key front() const { return keys_.front(); }
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); }
/** Construct 5-way factor */ /// Last key
Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4, KeyType key5) : keys_(5) { Key back() const { return keys_.back(); }
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; assertInvariants(); }
/** Construct 6-way factor */ /// find
Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4, KeyType key5, KeyType key6) : keys_(6) { const_iterator find(Key key) const { return std::find(begin(), end(), key); }
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; keys_[5] = key6; assertInvariants(); }
/// @} /// Access the factor's involved variable keys
/// @name Advanced Constructors const std::vector<Key>& keys() const { return keys_; }
/// @{
/** Construct n-way factor */ /** Iterator at beginning of involved variable keys */
Factor(const std::set<KeyType>& keys) { const_iterator begin() const { return keys_.begin(); }
BOOST_FOREACH(const KeyType& key, keys) keys_.push_back(key);
assertInvariants();
}
/** Construct n-way factor */ /** Iterator at end of involved variable keys */
Factor(const std::vector<KeyType>& keys) : keys_(keys) { const_iterator end() const { return keys_.end(); }
assertInvariants();
}
/** Constructor from a collection of keys */ /**
template<class KEYITERATOR> Factor(KEYITERATOR beginKey, KEYITERATOR endKey) : * @return the number of variables involved in this factor
keys_(beginKey, endKey) { assertInvariants(); } */
size_t size() const { return keys_.size(); }
/// @} /// @}
#ifdef TRACK_ELIMINATE
/**
* eliminate the first variable involved in this factor
* @return a conditional on the eliminated variable
*/
template<class CONDITIONAL>
typename CONDITIONAL::shared_ptr eliminateFirst();
/** /// @name Testable
* eliminate the first nrFrontals frontal variables. /// @{
*/
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::shared_ptr eliminate(size_t nrFrontals = 1);
#endif
/// @name Standard Interface /// print
/// @{ void print(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// First key /// print only keys
KeyType front() const { return keys_.front(); } void printKeys(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// Last key protected:
KeyType back() const { return keys_.back(); } /// check equality
bool equals(const This& other, double tol = 1e-9) const;
/// find /// @}
const_iterator find(KeyType key) const { return std::find(begin(), end(), key); }
/// Access the factor's involved variable keys public:
const std::vector<KeyType>& keys() const { return keys_; } /// @name Advanced Interface
/// @{
/** iterators */ /** @return keys involved in this factor */
const_iterator begin() const { return keys_.begin(); } ///TODO: comment std::vector<Key>& keys() { return keys_; }
const_iterator end() const { return keys_.end(); } ///TODO: comment
/** /** Iterator at beginning of involved variable keys */
* @return the number of variables involved in this factor iterator begin() { return keys_.begin(); }
*/
size_t size() const { return keys_.size(); }
/// @} /** Iterator at end of involved variable keys */
/// @name Testable iterator end() { return keys_.end(); }
/// @{
/// print private:
void print(const std::string& s = "Factor", /** Serialization function */
const IndexFormatter& formatter = DefaultIndexFormatter) const; friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(keys_);
}
/// print only keys /// @}
void printKeys(const std::string& s = "Factor",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/// check equality };
bool equals(const This& other, double tol = 1e-9) const;
/// @}
/// @name Advanced Interface
/// @{
/**
* @return keys involved in this factor
*/
std::vector<KeyType>& keys() { return keys_; }
/** mutable iterators */
iterator begin() { return keys_.begin(); } ///TODO: comment
iterator end() { return keys_.end(); } ///TODO: comment
protected:
friend class JacobianFactor;
friend class HessianFactor;
/// Internal consistency check that is run frequently when in debug mode.
/// If NDEBUG is defined, this is empty and optimized out.
void assertInvariants() const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(keys_);
}
/// @}
};
} }
#include <gtsam/inference/Factor-inl.h>

14
gtsam/inference/FactorGraphUnordered-inst.h → gtsam/inference/FactorGraph-inst.h
View File

@ -23,9 +23,9 @@
#include <gtsam/base/FastSet.h> #include <gtsam/base/FastSet.h>
#include <gtsam/base/treeTraversal-inst.h> #include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/inference/VariableIndex.h> #include <gtsam/inference/VariableIndexOrdered.h>
#include <gtsam/inference/FactorGraphUnordered.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/BayesTreeUnordered.h> #include <gtsam/inference/BayesTree.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/bind.hpp> #include <boost/bind.hpp>
@ -37,7 +37,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
void FactorGraphUnordered<FACTOR>::print(const std::string& s, const KeyFormatter& formatter) const { void FactorGraph<FACTOR>::print(const std::string& s, const KeyFormatter& formatter) const {
std::cout << s << std::endl; std::cout << s << std::endl;
std::cout << "size: " << size() << std::endl; std::cout << "size: " << size() << std::endl;
for (size_t i = 0; i < factors_.size(); i++) { for (size_t i = 0; i < factors_.size(); i++) {
@ -50,7 +50,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
bool FactorGraphUnordered<FACTOR>::equals(const This& fg, double tol) const { bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
/** check whether the two factor graphs have the same number of factors_ */ /** check whether the two factor graphs have the same number of factors_ */
if (factors_.size() != fg.size()) return false; if (factors_.size() != fg.size()) return false;
@ -67,7 +67,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
size_t FactorGraphUnordered<FACTOR>::nrFactors() const { size_t FactorGraph<FACTOR>::nrFactors() const {
size_t size_ = 0; size_t size_ = 0;
BOOST_FOREACH(const sharedFactor& factor, factors_) BOOST_FOREACH(const sharedFactor& factor, factors_)
if (factor) size_++; if (factor) size_++;
@ -76,7 +76,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
FastSet<Key> FactorGraphUnordered<FACTOR>::keys() const { FastSet<Key> FactorGraph<FACTOR>::keys() const {
FastSet<Key> allKeys; FastSet<Key> allKeys;
BOOST_FOREACH(const sharedFactor& factor, factors_) BOOST_FOREACH(const sharedFactor& factor, factors_)
if (factor) if (factor)

337
gtsam/inference/FactorGraph.h
View File

@ -15,25 +15,59 @@
* @author Carlos Nieto * @author Carlos Nieto
* @author Christian Potthast * @author Christian Potthast
* @author Michael Kaess * @author Michael Kaess
* @author Richard Roberts
*/ */
// \callgraph // \callgraph
#pragma once #pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/BayesNet.h>
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <boost/function.hpp> #include <boost/assign/list_inserter.hpp>
#include <set> #include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <type_traits>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
template<class CONDITIONAL, class CLIQUE> class BayesTree; template<class CLIQUE> class BayesTree;
class VariableIndex;
/** Helper */
template<class C>
class CRefCallPushBack
{
C& obj;
public:
CRefCallPushBack(C& obj) : obj(obj) {}
template<typename A>
void operator()(const A& a) { obj.push_back(a); }
};
/** Helper */
template<class C>
class RefCallPushBack
{
C& obj;
public:
RefCallPushBack(C& obj) : obj(obj) {}
template<typename A>
void operator()(A& a) { obj.push_back(a); }
};
/** Helper */
template<class C>
class CRefCallAddCopy
{
C& obj;
public:
CRefCallAddCopy(C& obj) : obj(obj) {}
template<typename A>
void operator()(const A& a) { obj.addCopy(a); }
};
/** /**
* A factor graph is a bipartite graph with factor nodes connected to variable nodes. * A factor graph is a bipartite graph with factor nodes connected to variable nodes.
@ -44,62 +78,64 @@ class VariableIndex;
class FactorGraph { class FactorGraph {
public: public:
typedef FACTOR FactorType; ///< factor type typedef FACTOR FactorType; ///< factor type
typedef typename FACTOR::KeyType KeyType; ///< type of Keys we use to index variables with
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
typedef boost::shared_ptr<typename FACTOR::ConditionalType> sharedConditional; ///< Shared pointer to a conditional typedef sharedFactor value_type;
typedef FactorGraph<FACTOR> This; ///< Typedef for this class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer for this class
typedef typename std::vector<sharedFactor>::iterator iterator; typedef typename std::vector<sharedFactor>::iterator iterator;
typedef typename std::vector<sharedFactor>::const_iterator const_iterator; typedef typename std::vector<sharedFactor>::const_iterator const_iterator;
/** typedef for elimination result */ private:
typedef std::pair<sharedConditional, sharedFactor> EliminationResult; typedef FactorGraph<FACTOR> This; ///< Typedef for this class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer for this class
/** typedef for an eliminate subroutine */
typedef boost::function<EliminationResult(const This&, size_t)> Eliminate;
protected: protected:
/** concept check, makes sure FACTOR defines print and equals */ /** concept check, makes sure FACTOR defines print and equals */
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
/** Collection of factors */ /** Collection of factors */
std::vector<sharedFactor> factors_; std::vector<sharedFactor> factors_;
public: /// @name Standard Constructors
/// @name Standard Constructor
/// @{ /// @{
/** Default constructor */ /** Default constructor */
FactorGraph() {} FactorGraph() {}
/** Constructor from iterator over factors (shared_ptr or plain objects) */
template<typename ITERATOR>
FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) { push_back(firstFactor, lastFactor); }
/** Construct from container of factors (shared_ptr or plain objects) */
template<class CONTAINER>
explicit FactorGraph(const CONTAINER& factors) { push_back(factors); }
/// @} /// @}
/// @name Advanced Constructors /// @name Advanced Constructors
/// @{ /// @{
// TODO: are these needed?
/** ///**
* @brief Constructor from a Bayes net // * @brief Constructor from a Bayes net
* @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor // * @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor
* @return a factor graph with all the conditionals, as factors // * @return a factor graph with all the conditionals, as factors
*/ // */
template<class CONDITIONAL> //template<class CONDITIONAL>
FactorGraph(const BayesNet<CONDITIONAL>& bayesNet); //FactorGraph(const BayesNet<CONDITIONAL>& bayesNet);
/** convert from Bayes tree */ ///** convert from Bayes tree */
template<class CONDITIONAL, class CLIQUE> //template<class CONDITIONAL, class CLIQUE>
FactorGraph(const BayesTree<CONDITIONAL, CLIQUE>& bayesTree); //FactorGraph(const BayesTree<CONDITIONAL, CLIQUE>& bayesTree);
/** convert from a derived type */ ///** convert from a derived type */
template<class DERIVEDFACTOR> //template<class DERIVEDFACTOR>
FactorGraph(const FactorGraph<DERIVEDFACTOR>& factors) { //FactorGraph(const FactorGraph<DERIVEDFACTOR>& factors) {
factors_.assign(factors.begin(), factors.end()); // factors_.assign(factors.begin(), factors.end());
} //}
/// @} /// @}
public:
/// @name Adding Factors /// @name Adding Factors
/// @{ /// @{
@ -109,69 +145,159 @@ class VariableIndex;
*/ */
void reserve(size_t size) { factors_.reserve(size); } void reserve(size_t size) { factors_.reserve(size); }
/** Add a factor */ // TODO: are these needed?
/** Add a factor directly using a shared_ptr */
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) { typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
factors_.push_back(boost::shared_ptr<FACTOR>(factor)); push_back(boost::shared_ptr<DERIVEDFACTOR>& factor) {
} factors_.push_back(boost::shared_ptr<FACTOR>(factor)); }
/** push back many factors */ /** Add a factor directly using a shared_ptr */
void push_back(const This& factors) { void push_back(const sharedFactor& factor) {
factors_.insert(end(), factors.begin(), factors.end()); factors_.push_back(factor); }
}
/** push back many factors with an iterator */ /** push back many factors with an iterator over shared_ptr (factors are not copied) */
template<typename ITERATOR> template<typename ITERATOR>
void push_back(ITERATOR firstFactor, ITERATOR lastFactor) { typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type::element_type>::value>::type
factors_.insert(end(), firstFactor, lastFactor); push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
factors_.insert(end(), firstFactor, lastFactor); }
/** push back many factors as shared_ptr's in a container (factors are not copied) */
template<typename CONTAINER>
typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type::element_type>::value>::type
push_back(const CONTAINER& container) {
push_back(container.begin(), container.end());
} }
/** /** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived
* @brief Add a vector of derived factors * classes in favor of a type-specialized version that calls this templated function. */
* @param factors to add template<class CLIQUE>
*/ typename std::enable_if<std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
template<typename DERIVEDFACTOR> push_back(const BayesTree<CLIQUE>& bayesTree) {
void push_back(const std::vector<typename boost::shared_ptr<DERIVEDFACTOR> >& factors) { bayesTree.addFactorsToGraph(*this);
factors_.insert(end(), factors.begin(), factors.end());
} }
/** += syntax for push_back, e.g. graph += f1, f2, f3 */
//template<class T>
//boost::assign::list_inserter<boost::function<void(const T&)> >
// operator+=(const T& factorOrContainer)
//{
// return boost::assign::make_list_inserter(
// boost::bind(&This::push_back<T>, this, _1));
//}
/** Add a factor directly using a shared_ptr */
template<class DERIVEDFACTOR>
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value,
boost::assign::list_inserter<RefCallPushBack<This> > >::type
operator+=(boost::shared_ptr<DERIVEDFACTOR>& factor) {
return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(factor);
}
template<class FACTOR_OR_CONTAINER>
boost::assign::list_inserter<CRefCallPushBack<This> >
operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) {
return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(factorOrContainer);
}
///** Add a factor directly using a shared_ptr */
//boost::assign::list_inserter<CRefCallPushBack<This> >
// operator+=(const sharedFactor& factor) {
// return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this));
//}
///** push back many factors as shared_ptr's in a container (factors are not copied) */
//template<typename CONTAINER>
//typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type::element_type>::value,
// boost::assign::list_inserter<CRefCallPushBack<This> > >::type
// operator+=(const CONTAINER& container) {
// return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this));
//}
///** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived
// * classes in favor of a type-specialized version that calls this templated function. */
//template<class CLIQUE>
//boost::assign::list_inserter<CRefCallPushBack<This> >
// operator+=(const BayesTree<CLIQUE>& bayesTree) {
// return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this));
//}
/** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid
* the copy). */
template<class DERIVEDFACTOR>
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
push_back(const DERIVEDFACTOR& factor) {
factors_.push_back(boost::make_shared<DERIVEDFACTOR>(factor)); }
/** push back many factors with an iterator over plain factors (factors are copied) */
template<typename ITERATOR>
typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type>::value>::type
push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
for(ITERATOR f = firstFactor; f != lastFactor; ++f)
push_back(*f);
}
/** push back many factors as non-pointer objects in a container (factors are copied) */
template<typename CONTAINER>
typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type>::value>::type
push_back(const CONTAINER& container) {
push_back(container.begin(), container.end());
}
//template<class FACTOR_OR_CONTAINER>
//boost::assign::list_inserter<CRefCallPushBack<This> >
// operator*=(const FACTOR_OR_CONTAINER& factorOrContainer) {
// return boost::assign::make_list_inserter(CRefCallAddCopy<This>(*this));
//}
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
/** print out graph */ /** print out graph */
void print(const std::string& s = "FactorGraph", void print(const std::string& s = "FactorGraph",
const IndexFormatter& formatter = DefaultIndexFormatter) const; const KeyFormatter& formatter = DefaultKeyFormatter) const;
protected:
/** Check equality */ /** Check equality */
bool equals(const This& fg, double tol = 1e-9) const; bool equals(const This& fg, double tol = 1e-9) const;
/// @} /// @}
public:
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/** return the number of factors and NULLS */ /** return the number of factors (including any null factors set by remove() ). */
size_t size() const { return factors_.size();} size_t size() const { return factors_.size(); }
/** Simple check for an empty graph - faster than comparing size() to zero */ /** Check if the graph is empty (null factors set by remove() will cause this to return false). */
bool empty() const { return factors_.empty(); } bool empty() const { return factors_.empty(); }
/** const cast to the underlying vector of factors */ /** Get a specific factor by index (this checks array bounds and may throw an exception, as
operator const std::vector<sharedFactor>&() const { return factors_; } * opposed to operator[] which does not).
*/
const sharedFactor at(size_t i) const { return factors_.at(i); }
/** Get a specific factor by index */ /** Get a specific factor by index (this checks array bounds and may throw an exception, as
const sharedFactor at(size_t i) const { assert(i<factors_.size()); return factors_[i]; } * opposed to operator[] which does not).
sharedFactor& at(size_t i) { assert(i<factors_.size()); return factors_[i]; } */
sharedFactor& at(size_t i) { return factors_.at(i); }
/** Get a specific factor by index (this does not check array bounds, as opposed to at() which
* does).
*/
const sharedFactor operator[](size_t i) const { return at(i); } const sharedFactor operator[](size_t i) const { return at(i); }
/** Get a specific factor by index (this does not check array bounds, as opposed to at() which
* does).
*/
sharedFactor& operator[](size_t i) { return at(i); } sharedFactor& operator[](size_t i) { return at(i); }
/** Checks whether a valid factor exists at the given index */ /** Iterator to beginning of factors. */
inline bool exists(size_t i) const { return i<factors_.size() && factors_[i]; }
/** STL begin, so we can use BOOST_FOREACH */
const_iterator begin() const { return factors_.begin();} const_iterator begin() const { return factors_.begin();}
/** STL end, so we can use BOOST_FOREACH */ /** Iterator to end of factors. */
const_iterator end() const { return factors_.end(); } const_iterator end() const { return factors_.end(); }
/** Get the first factor */ /** Get the first factor */
@ -180,38 +306,6 @@ class VariableIndex;
/** Get the last factor */ /** Get the last factor */
sharedFactor back() const { return factors_.back(); } sharedFactor back() const { return factors_.back(); }
/** Eliminate the first \c n frontal variables, returning the resulting
* conditional and remaining factor graph - this is very inefficient for
* eliminating all variables, to do that use EliminationTree or
* JunctionTree.
*/
std::pair<sharedConditional, FactorGraph<FactorType> > eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const;
/** Factor the factor graph into a conditional and a remaining factor graph.
* Given the factor graph \f$ f(X) \f$, and \c variables to factorize out
* \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where
* \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is
* a probability density or likelihood, the factorization produces a
* conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$.
*
* For efficiency, this function treats the variables to eliminate
* \c variables as fully-connected, so produces a dense (fully-connected)
* conditional on all of the variables in \c variables, instead of a sparse
* BayesNet. If the variables are not fully-connected, it is more efficient
* to sequentially factorize multiple times.
*/
std::pair<sharedConditional, FactorGraph<FactorType> > eliminate(
const std::vector<KeyType>& variables, const Eliminate& eliminateFcn,
boost::optional<const VariableIndex&> variableIndex = boost::none) const;
/** Eliminate a single variable, by calling FactorGraph::eliminate. */
std::pair<sharedConditional, FactorGraph<FactorType> > eliminateOne(
KeyType variable, const Eliminate& eliminateFcn,
boost::optional<const VariableIndex&> variableIndex = boost::none) const {
std::vector<size_t> variables(1, variable);
return eliminate(variables, eliminateFcn, variableIndex);
}
/// @} /// @}
/// @name Modifying Factor Graphs (imperative, discouraged) /// @name Modifying Factor Graphs (imperative, discouraged)
/// @{ /// @{
@ -222,24 +316,33 @@ class VariableIndex;
/** non-const STL-style end() */ /** non-const STL-style end() */
iterator end() { return factors_.end(); } iterator end() { return factors_.end(); }
/** resize the factor graph. TODO: effects? */ /** Directly resize the number of factors in the graph. If the new size is less than the
* original, factors at the end will be removed. If the new size is larger than the original,
* null factors will be appended.
*/
void resize(size_t size) { factors_.resize(size); } void resize(size_t size) { factors_.resize(size); }
/** delete factor without re-arranging indexes by inserting a NULL pointer */ /** delete factor without re-arranging indexes by inserting a NULL pointer */
inline void remove(size_t i) { factors_[i].reset();} void remove(size_t i) { factors_[i].reset();}
/** replace a factor by index */ /** replace a factor by index */
void replace(size_t index, sharedFactor factor); void replace(size_t index, sharedFactor factor) { at(index) = factor; }
/** Erase factor and rearrange other factors to take up the empty space */
void erase(const_iterator item) { factors_.erase(item); }
/** Erase factors and rearrange other factors to take up the empty space */
void erase(const_iterator first, const_iterator last) { factors_.erase(first, last); }
/// @} /// @}
/// @name Advanced Interface /// @name Advanced Interface
/// @{ /// @{
/** return the number valid factors */ /** return the number of non-null factors */
size_t nrFactors() const; size_t nrFactors() const;
/** Potentially very slow function to return all keys involved */ /** Potentially very slow function to return all keys involved */
std::set<KeyType> keys() const; FastSet<Key> keys() const;
private: private:
@ -254,20 +357,4 @@ class VariableIndex;
}; // FactorGraph }; // FactorGraph
/** Create a combined joint factor (new style for EliminationTree). */
template<class DERIVEDFACTOR, class KEY>
typename DERIVEDFACTOR::shared_ptr Combine(const FactorGraph<DERIVEDFACTOR>& factors,
const FastMap<KEY, std::vector<KEY> >& variableSlots);
/**
* static function that combines two factor graphs
* @param fg1 Linear factor graph
* @param fg2 Linear factor graph
* @return a new combined factor graph
*/
template<class FACTORGRAPH>
FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2);
} // namespace gtsam } // namespace gtsam
#include <gtsam/inference/FactorGraph-inl.h>

50
gtsam/inference/FactorGraph-inl.h → gtsam/inference/FactorGraphOrdered-inl.h
View File

@ -23,8 +23,8 @@
#pragma once #pragma once
#include <gtsam/base/FastSet.h> #include <gtsam/base/FastSet.h>
#include <gtsam/inference/BayesTree.h> #include <gtsam/inference/BayesTreeOrdered.h>
#include <gtsam/inference/VariableIndex.h> #include <gtsam/inference/VariableIndexOrdered.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
@ -41,7 +41,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
template<class CONDITIONAL> template<class CONDITIONAL>
FactorGraph<FACTOR>::FactorGraph(const BayesNet<CONDITIONAL>& bayesNet) { FactorGraphOrdered<FACTOR>::FactorGraphOrdered(const BayesNetOrdered<CONDITIONAL>& bayesNet) {
factors_.reserve(bayesNet.size()); factors_.reserve(bayesNet.size());
BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, bayesNet) { BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, bayesNet) {
this->push_back(cond->toFactor()); this->push_back(cond->toFactor());
@ -50,7 +50,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
void FactorGraph<FACTOR>::print(const std::string& s, void FactorGraphOrdered<FACTOR>::print(const std::string& s,
const IndexFormatter& formatter) const { const IndexFormatter& formatter) const {
std::cout << s << std::endl; std::cout << s << std::endl;
std::cout << "size: " << size() << std::endl; std::cout << "size: " << size() << std::endl;
@ -63,7 +63,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const { bool FactorGraphOrdered<FACTOR>::equals(const This& fg, double tol) const {
/** check whether the two factor graphs have the same number of factors_ */ /** check whether the two factor graphs have the same number of factors_ */
if (factors_.size() != fg.size()) return false; if (factors_.size() != fg.size()) return false;
@ -80,7 +80,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
size_t FactorGraph<FACTOR>::nrFactors() const { size_t FactorGraphOrdered<FACTOR>::nrFactors() const {
size_t size_ = 0; size_t size_ = 0;
BOOST_FOREACH(const sharedFactor& factor, factors_) BOOST_FOREACH(const sharedFactor& factor, factors_)
if (factor) size_++; if (factor) size_++;
@ -89,7 +89,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
std::set<typename FACTOR::KeyType> FactorGraph<FACTOR>::keys() const { std::set<typename FACTOR::KeyType> FactorGraphOrdered<FACTOR>::keys() const {
std::set<KeyType> allKeys; std::set<KeyType> allKeys;
BOOST_FOREACH(const sharedFactor& factor, factors_) BOOST_FOREACH(const sharedFactor& factor, factors_)
if (factor) { if (factor) {
@ -101,11 +101,11 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
std::pair<typename FactorGraph<FACTOR>::sharedConditional, FactorGraph<FACTOR> > std::pair<typename FactorGraphOrdered<FACTOR>::sharedConditional, FactorGraphOrdered<FACTOR> >
FactorGraph<FACTOR>::eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const FactorGraphOrdered<FACTOR>::eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const
{ {
// Build variable index // Build variable index
VariableIndex variableIndex(*this); VariableIndexOrdered variableIndex(*this);
// Find first variable // Find first variable
Index firstIndex = 0; Index firstIndex = 0;
@ -125,8 +125,8 @@ namespace gtsam {
} }
// Separate factors into involved and remaining // Separate factors into involved and remaining
FactorGraph<FactorType> involvedFactors; FactorGraphOrdered<FactorType> involvedFactors;
FactorGraph<FactorType> remainingFactors; FactorGraphOrdered<FactorType> remainingFactors;
FastSet<size_t>::const_iterator involvedFactorIsIt = involvedFactorIs.begin(); FastSet<size_t>::const_iterator involvedFactorIsIt = involvedFactorIs.begin();
for(size_t i = 0; i < this->size(); ++i) { for(size_t i = 0; i < this->size(); ++i) {
if(involvedFactorIsIt != involvedFactorIs.end() && *involvedFactorIsIt == i) { if(involvedFactorIsIt != involvedFactorIs.end() && *involvedFactorIsIt == i) {
@ -140,7 +140,7 @@ namespace gtsam {
} }
// Do dense elimination on the involved factors // Do dense elimination on the involved factors
typename FactorGraph<FactorType>::EliminationResult eliminationResult = typename FactorGraphOrdered<FactorType>::EliminationResult eliminationResult =
eliminate(involvedFactors, nFrontals); eliminate(involvedFactors, nFrontals);
// Add the remaining factor back into the factor graph // Add the remaining factor back into the factor graph
@ -152,15 +152,15 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
std::pair<typename FactorGraph<FACTOR>::sharedConditional, FactorGraph<FACTOR> > std::pair<typename FactorGraphOrdered<FACTOR>::sharedConditional, FactorGraphOrdered<FACTOR> >
FactorGraph<FACTOR>::eliminate(const std::vector<KeyType>& variables, const Eliminate& eliminateFcn, FactorGraphOrdered<FACTOR>::eliminate(const std::vector<KeyType>& variables, const Eliminate& eliminateFcn,
boost::optional<const VariableIndex&> variableIndex_) const boost::optional<const VariableIndexOrdered&> variableIndex_) const
{ {
const VariableIndex& variableIndex = const VariableIndexOrdered& variableIndex =
variableIndex_ ? *variableIndex_ : VariableIndex(*this); variableIndex_ ? *variableIndex_ : VariableIndexOrdered(*this);
// First find the involved factors // First find the involved factors
FactorGraph<FACTOR> involvedFactors; FactorGraphOrdered<FACTOR> involvedFactors;
Index highestInvolvedVariable = 0; // Largest index of the variables in the involved factors Index highestInvolvedVariable = 0; // Largest index of the variables in the involved factors
// First get the indices of the involved factors, but uniquely in a set // First get the indices of the involved factors, but uniquely in a set
@ -205,7 +205,7 @@ namespace gtsam {
} }
// Build the remaining graph, without the removed factors // Build the remaining graph, without the removed factors
FactorGraph<FACTOR> remainingGraph; FactorGraphOrdered<FACTOR> remainingGraph;
remainingGraph.reserve(this->size() - involvedFactors.size() + 1); remainingGraph.reserve(this->size() - involvedFactors.size() + 1);
FastSet<size_t>::const_iterator involvedFactorIndexIt = involvedFactorIndices.begin(); FastSet<size_t>::const_iterator involvedFactorIndexIt = involvedFactorIndices.begin();
for(size_t i = 0; i < this->size(); ++i) { for(size_t i = 0; i < this->size(); ++i) {
@ -224,7 +224,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
void FactorGraph<FACTOR>::replace(size_t index, sharedFactor factor) { void FactorGraphOrdered<FACTOR>::replace(size_t index, sharedFactor factor) {
if (index >= factors_.size()) throw std::invalid_argument(boost::str( if (index >= factors_.size()) throw std::invalid_argument(boost::str(
boost::format("Factor graph does not contain a factor with index %d.") boost::format("Factor graph does not contain a factor with index %d.")
% index)); % index));
@ -246,7 +246,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVEDFACTOR, class KEY> template<class DERIVEDFACTOR, class KEY>
typename DERIVEDFACTOR::shared_ptr Combine(const FactorGraph<DERIVEDFACTOR>& factors, typename DERIVEDFACTOR::shared_ptr Combine(const FactorGraphOrdered<DERIVEDFACTOR>& factors,
const FastMap<KEY, std::vector<KEY> >& variableSlots) { const FastMap<KEY, std::vector<KEY> >& variableSlots) {
typedef const std::pair<const KEY, std::vector<KEY> > KeySlotPair; typedef const std::pair<const KEY, std::vector<KEY> > KeySlotPair;
@ -263,14 +263,14 @@ namespace gtsam {
template<class FACTOR, class CONDITIONAL, class CLIQUE> template<class FACTOR, class CONDITIONAL, class CLIQUE>
void _FactorGraph_BayesTree_adder( void _FactorGraph_BayesTree_adder(
std::vector<typename boost::shared_ptr<FACTOR> >& factors_io, std::vector<typename boost::shared_ptr<FACTOR> >& factors_io,
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) { const typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique& clique) {
if(clique) { if(clique) {
// Add factor from this clique // Add factor from this clique
factors_io.push_back((*clique)->toFactor()); factors_io.push_back((*clique)->toFactor());
// Traverse children // Traverse children
typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique; typedef typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& child, clique->children()) BOOST_FOREACH(const sharedClique& child, clique->children())
_FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors_io, child); _FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors_io, child);
} }
@ -279,7 +279,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
FactorGraph<FACTOR>::FactorGraph(const BayesTree<CONDITIONAL,CLIQUE>& bayesTree) { FactorGraphOrdered<FACTOR>::FactorGraphOrdered(const BayesTreeOrdered<CONDITIONAL,CLIQUE>& bayesTree) {
factors_.reserve(bayesTree.size()); factors_.reserve(bayesTree.size());
_FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors_, bayesTree.root()); _FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors_, bayesTree.root());
} }

273
gtsam/inference/FactorGraphOrdered.h Normal file
View File

@ -0,0 +1,273 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file FactorGraph.h
* @brief Factor Graph Base Class
* @author Carlos Nieto
* @author Christian Potthast
* @author Michael Kaess
*/
// \callgraph
#pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/BayesNetOrdered.h>
#include <boost/serialization/nvp.hpp>
#include <boost/function.hpp>
#include <set>
namespace gtsam {
// Forward declarations
template<class CONDITIONAL, class CLIQUE> class BayesTreeOrdered;
class VariableIndexOrdered;
/**
* A factor graph is a bipartite graph with factor nodes connected to variable nodes.
* In this class, however, only factor nodes are kept around.
* \nosubgrouping
*/
template<class FACTOR>
class FactorGraphOrdered {
public:
typedef FACTOR FactorType; ///< factor type
typedef typename FACTOR::KeyType KeyType; ///< type of Keys we use to index variables with
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
typedef boost::shared_ptr<typename FACTOR::ConditionalType> sharedConditional; ///< Shared pointer to a conditional
typedef FactorGraphOrdered<FACTOR> This; ///< Typedef for this class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer for this class
typedef typename std::vector<sharedFactor>::iterator iterator;
typedef typename std::vector<sharedFactor>::const_iterator const_iterator;
/** typedef for elimination result */
typedef std::pair<sharedConditional, sharedFactor> EliminationResult;
/** typedef for an eliminate subroutine */
typedef boost::function<EliminationResult(const This&, size_t)> Eliminate;
protected:
/** concept check, makes sure FACTOR defines print and equals */
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
/** Collection of factors */
std::vector<sharedFactor> factors_;
public:
/// @name Standard Constructor
/// @{
/** Default constructor */
FactorGraphOrdered() {}
/// @}
/// @name Advanced Constructors
/// @{
/**
* @brief Constructor from a Bayes net
* @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor
* @return a factor graph with all the conditionals, as factors
*/
template<class CONDITIONAL>
FactorGraphOrdered(const BayesNetOrdered<CONDITIONAL>& bayesNet);
/** convert from Bayes tree */
template<class CONDITIONAL, class CLIQUE>
FactorGraphOrdered(const BayesTreeOrdered<CONDITIONAL, CLIQUE>& bayesTree);
/** convert from a derived type */
template<class DERIVEDFACTOR>
FactorGraphOrdered(const FactorGraphOrdered<DERIVEDFACTOR>& factors) {
factors_.assign(factors.begin(), factors.end());
}
/// @}
/// @name Adding Factors
/// @{
/**
* Reserve space for the specified number of factors if you know in
* advance how many there will be (works like std::vector::reserve).
*/
void reserve(size_t size) { factors_.reserve(size); }
/** Add a factor */
template<class DERIVEDFACTOR>
void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) {
factors_.push_back(boost::shared_ptr<FACTOR>(factor));
}
/** push back many factors */
void push_back(const This& factors) {
factors_.insert(end(), factors.begin(), factors.end());
}
/** push back many factors with an iterator */
template<typename ITERATOR>
void push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
factors_.insert(end(), firstFactor, lastFactor);
}
/**
* @brief Add a vector of derived factors
* @param factors to add
*/
template<typename DERIVEDFACTOR>
void push_back(const std::vector<typename boost::shared_ptr<DERIVEDFACTOR> >& factors) {
factors_.insert(end(), factors.begin(), factors.end());
}
/// @}
/// @name Testable
/// @{
/** print out graph */
void print(const std::string& s = "FactorGraph",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** Check equality */
bool equals(const This& fg, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
/** return the number of factors and NULLS */
size_t size() const { return factors_.size();}
/** Simple check for an empty graph - faster than comparing size() to zero */
bool empty() const { return factors_.empty(); }
/** const cast to the underlying vector of factors */
operator const std::vector<sharedFactor>&() const { return factors_; }
/** Get a specific factor by index */
const sharedFactor at(size_t i) const { assert(i<factors_.size()); return factors_[i]; }
sharedFactor& at(size_t i) { assert(i<factors_.size()); return factors_[i]; }
const sharedFactor operator[](size_t i) const { return at(i); }
sharedFactor& operator[](size_t i) { return at(i); }
/** Checks whether a valid factor exists at the given index */
inline bool exists(size_t i) const { return i<factors_.size() && factors_[i]; }
/** STL begin, so we can use BOOST_FOREACH */
const_iterator begin() const { return factors_.begin();}
/** STL end, so we can use BOOST_FOREACH */
const_iterator end() const { return factors_.end(); }
/** Get the first factor */
sharedFactor front() const { return factors_.front(); }
/** Get the last factor */
sharedFactor back() const { return factors_.back(); }
/** Eliminate the first \c n frontal variables, returning the resulting
* conditional and remaining factor graph - this is very inefficient for
* eliminating all variables, to do that use EliminationTree or
* JunctionTree.
*/
std::pair<sharedConditional, FactorGraphOrdered<FactorType> > eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const;
/** Factor the factor graph into a conditional and a remaining factor graph.
* Given the factor graph \f$ f(X) \f$, and \c variables to factorize out
* \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where
* \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is
* a probability density or likelihood, the factorization produces a
* conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$.
*
* For efficiency, this function treats the variables to eliminate
* \c variables as fully-connected, so produces a dense (fully-connected)
* conditional on all of the variables in \c variables, instead of a sparse
* BayesNet. If the variables are not fully-connected, it is more efficient
* to sequentially factorize multiple times.
*/
std::pair<sharedConditional, FactorGraphOrdered<FactorType> > eliminate(
const std::vector<KeyType>& variables, const Eliminate& eliminateFcn,
boost::optional<const VariableIndexOrdered&> variableIndex = boost::none) const;
/** Eliminate a single variable, by calling FactorGraph::eliminate. */
std::pair<sharedConditional, FactorGraphOrdered<FactorType> > eliminateOne(
KeyType variable, const Eliminate& eliminateFcn,
boost::optional<const VariableIndexOrdered&> variableIndex = boost::none) const {
std::vector<size_t> variables(1, variable);
return eliminate(variables, eliminateFcn, variableIndex);
}
/// @}
/// @name Modifying Factor Graphs (imperative, discouraged)
/// @{
/** non-const STL-style begin() */
iterator begin() { return factors_.begin();}
/** non-const STL-style end() */
iterator end() { return factors_.end(); }
/** resize the factor graph. TODO: effects? */
void resize(size_t size) { factors_.resize(size); }
/** delete factor without re-arranging indexes by inserting a NULL pointer */
inline void remove(size_t i) { factors_[i].reset();}
/** replace a factor by index */
void replace(size_t index, sharedFactor factor);
/// @}
/// @name Advanced Interface
/// @{
/** return the number valid factors */
size_t nrFactors() const;
/** Potentially very slow function to return all keys involved */
std::set<KeyType> keys() const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(factors_);
}
/// @}
}; // FactorGraph
/** Create a combined joint factor (new style for EliminationTree). */
template<class DERIVEDFACTOR, class KEY>
typename DERIVEDFACTOR::shared_ptr Combine(const FactorGraphOrdered<DERIVEDFACTOR>& factors,
const FastMap<KEY, std::vector<KEY> >& variableSlots);
/**
* static function that combines two factor graphs
* @param fg1 Linear factor graph
* @param fg2 Linear factor graph
* @return a new combined factor graph
*/
template<class FACTORGRAPH>
FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2);
} // namespace gtsam
#include <gtsam/inference/FactorGraphOrdered-inl.h>

360
gtsam/inference/FactorGraphUnordered.h
View File

@ -1,360 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file FactorGraph.h
* @brief Factor Graph Base Class
* @author Carlos Nieto
* @author Christian Potthast
* @author Michael Kaess
* @author Richard Roberts
*/
// \callgraph
#pragma once
#include <boost/serialization/nvp.hpp>
#include <boost/assign/list_inserter.hpp>
#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <type_traits>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
namespace gtsam {
// Forward declarations
template<class CLIQUE> class BayesTreeUnordered;
/** Helper */
template<class C>
class CRefCallPushBack
{
C& obj;
public:
CRefCallPushBack(C& obj) : obj(obj) {}
template<typename A>
void operator()(const A& a) { obj.push_back(a); }
};
/** Helper */
template<class C>
class RefCallPushBack
{
C& obj;
public:
RefCallPushBack(C& obj) : obj(obj) {}
template<typename A>
void operator()(A& a) { obj.push_back(a); }
};
/** Helper */
template<class C>
class CRefCallAddCopy
{
C& obj;
public:
CRefCallAddCopy(C& obj) : obj(obj) {}
template<typename A>
void operator()(const A& a) { obj.addCopy(a); }
};
/**
* A factor graph is a bipartite graph with factor nodes connected to variable nodes.
* In this class, however, only factor nodes are kept around.
* \nosubgrouping
*/
template<class FACTOR>
class FactorGraphUnordered {
public:
typedef FACTOR FactorType; ///< factor type
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
typedef sharedFactor value_type;
typedef typename std::vector<sharedFactor>::iterator iterator;
typedef typename std::vector<sharedFactor>::const_iterator const_iterator;
private:
typedef FactorGraphUnordered<FACTOR> This; ///< Typedef for this class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer for this class
protected:
/** concept check, makes sure FACTOR defines print and equals */
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
/** Collection of factors */
std::vector<sharedFactor> factors_;
/// @name Standard Constructors
/// @{
/** Default constructor */
FactorGraphUnordered() {}
/** Constructor from iterator over factors (shared_ptr or plain objects) */
template<typename ITERATOR>
FactorGraphUnordered(ITERATOR firstFactor, ITERATOR lastFactor) { push_back(firstFactor, lastFactor); }
/** Construct from container of factors (shared_ptr or plain objects) */
template<class CONTAINER>
explicit FactorGraphUnordered(const CONTAINER& factors) { push_back(factors); }
/// @}
/// @name Advanced Constructors
/// @{
// TODO: are these needed?
///**
// * @brief Constructor from a Bayes net
// * @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor
// * @return a factor graph with all the conditionals, as factors
// */
//template<class CONDITIONAL>
//FactorGraph(const BayesNet<CONDITIONAL>& bayesNet);
///** convert from Bayes tree */
//template<class CONDITIONAL, class CLIQUE>
//FactorGraph(const BayesTree<CONDITIONAL, CLIQUE>& bayesTree);
///** convert from a derived type */
//template<class DERIVEDFACTOR>
//FactorGraph(const FactorGraph<DERIVEDFACTOR>& factors) {
// factors_.assign(factors.begin(), factors.end());
//}
/// @}
public:
/// @name Adding Factors
/// @{
/**
* Reserve space for the specified number of factors if you know in
* advance how many there will be (works like std::vector::reserve).
*/
void reserve(size_t size) { factors_.reserve(size); }
// TODO: are these needed?
/** Add a factor directly using a shared_ptr */
template<class DERIVEDFACTOR>
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
push_back(boost::shared_ptr<DERIVEDFACTOR>& factor) {
factors_.push_back(boost::shared_ptr<FACTOR>(factor)); }
/** Add a factor directly using a shared_ptr */
void push_back(const sharedFactor& factor) {
factors_.push_back(factor); }
/** push back many factors with an iterator over shared_ptr (factors are not copied) */
template<typename ITERATOR>
typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type::element_type>::value>::type
push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
factors_.insert(end(), firstFactor, lastFactor); }
/** push back many factors as shared_ptr's in a container (factors are not copied) */
template<typename CONTAINER>
typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type::element_type>::value>::type
push_back(const CONTAINER& container) {
push_back(container.begin(), container.end());
}
/** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived
* classes in favor of a type-specialized version that calls this templated function. */
template<class CLIQUE>
typename std::enable_if<std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
push_back(const BayesTreeUnordered<CLIQUE>& bayesTree) {
bayesTree.addFactorsToGraph(*this);
}
/** += syntax for push_back, e.g. graph += f1, f2, f3 */
//template<class T>
//boost::assign::list_inserter<boost::function<void(const T&)> >
// operator+=(const T& factorOrContainer)
//{
// return boost::assign::make_list_inserter(
// boost::bind(&This::push_back<T>, this, _1));
//}
/** Add a factor directly using a shared_ptr */
template<class DERIVEDFACTOR>
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value,
boost::assign::list_inserter<RefCallPushBack<This> > >::type
operator+=(boost::shared_ptr<DERIVEDFACTOR>& factor) {
return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(factor);
}
template<class FACTOR_OR_CONTAINER>
boost::assign::list_inserter<CRefCallPushBack<This> >
operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) {
return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(factorOrContainer);
}
///** Add a factor directly using a shared_ptr */
//boost::assign::list_inserter<CRefCallPushBack<This> >
// operator+=(const sharedFactor& factor) {
// return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this));
//}
///** push back many factors as shared_ptr's in a container (factors are not copied) */
//template<typename CONTAINER>
//typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type::element_type>::value,
// boost::assign::list_inserter<CRefCallPushBack<This> > >::type
// operator+=(const CONTAINER& container) {
// return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this));
//}
///** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived
// * classes in favor of a type-specialized version that calls this templated function. */
//template<class CLIQUE>
//boost::assign::list_inserter<CRefCallPushBack<This> >
// operator+=(const BayesTreeUnordered<CLIQUE>& bayesTree) {
// return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this));
//}
/** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid
* the copy). */
template<class DERIVEDFACTOR>
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
push_back(const DERIVEDFACTOR& factor) {
factors_.push_back(boost::make_shared<DERIVEDFACTOR>(factor)); }
/** push back many factors with an iterator over plain factors (factors are copied) */
template<typename ITERATOR>
typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type>::value>::type
push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
for(ITERATOR f = firstFactor; f != lastFactor; ++f)
push_back(*f);
}
/** push back many factors as non-pointer objects in a container (factors are copied) */
template<typename CONTAINER>
typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type>::value>::type
push_back(const CONTAINER& container) {
push_back(container.begin(), container.end());
}
//template<class FACTOR_OR_CONTAINER>
//boost::assign::list_inserter<CRefCallPushBack<This> >
// operator*=(const FACTOR_OR_CONTAINER& factorOrContainer) {
// return boost::assign::make_list_inserter(CRefCallAddCopy<This>(*this));
//}
/// @}
/// @name Testable
/// @{
/** print out graph */
void print(const std::string& s = "FactorGraph",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
protected:
/** Check equality */
bool equals(const This& fg, double tol = 1e-9) const;
/// @}
public:
/// @name Standard Interface
/// @{
/** return the number of factors (including any null factors set by remove() ). */
size_t size() const { return factors_.size(); }
/** Check if the graph is empty (null factors set by remove() will cause this to return false). */
bool empty() const { return factors_.empty(); }
/** Get a specific factor by index (this checks array bounds and may throw an exception, as
* opposed to operator[] which does not).
*/
const sharedFactor at(size_t i) const { return factors_.at(i); }
/** Get a specific factor by index (this checks array bounds and may throw an exception, as
* opposed to operator[] which does not).
*/
sharedFactor& at(size_t i) { return factors_.at(i); }
/** Get a specific factor by index (this does not check array bounds, as opposed to at() which
* does).
*/
const sharedFactor operator[](size_t i) const { return at(i); }
/** Get a specific factor by index (this does not check array bounds, as opposed to at() which
* does).
*/
sharedFactor& operator[](size_t i) { return at(i); }
/** Iterator to beginning of factors. */
const_iterator begin() const { return factors_.begin();}
/** Iterator to end of factors. */
const_iterator end() const { return factors_.end(); }
/** Get the first factor */
sharedFactor front() const { return factors_.front(); }
/** Get the last factor */
sharedFactor back() const { return factors_.back(); }
/// @}
/// @name Modifying Factor Graphs (imperative, discouraged)
/// @{
/** non-const STL-style begin() */
iterator begin() { return factors_.begin();}
/** non-const STL-style end() */
iterator end() { return factors_.end(); }
/** Directly resize the number of factors in the graph. If the new size is less than the
* original, factors at the end will be removed. If the new size is larger than the original,
* null factors will be appended.
*/
void resize(size_t size) { factors_.resize(size); }
/** delete factor without re-arranging indexes by inserting a NULL pointer */
void remove(size_t i) { factors_[i].reset();}
/** replace a factor by index */
void replace(size_t index, sharedFactor factor) { at(index) = factor; }
/** Erase factor and rearrange other factors to take up the empty space */
void erase(const_iterator item) { factors_.erase(item); }
/** Erase factors and rearrange other factors to take up the empty space */
void erase(const_iterator first, const_iterator last) { factors_.erase(first, last); }
/// @}
/// @name Advanced Interface
/// @{
/** return the number of non-null factors */
size_t nrFactors() const;
/** Potentially very slow function to return all keys involved */
FastSet<Key> keys() const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(factors_);
}
/// @}
}; // FactorGraph
} // namespace gtsam

20
gtsam/inference/Factor-inl.h → gtsam/inference/FactorOrdered-inl.h
View File

@ -17,7 +17,7 @@
#pragma once #pragma once
#include <gtsam/inference/Factor.h> #include <gtsam/inference/FactorOrdered.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <iostream> #include <iostream>
@ -27,14 +27,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<typename KEY> template<typename KEY>
Factor<KEY>::Factor(const Factor<KEY>& f) : FactorOrdered<KEY>::FactorOrdered(const FactorOrdered<KEY>& f) :
keys_(f.keys_) { keys_(f.keys_) {
assertInvariants(); assertInvariants();
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<typename KEY> template<typename KEY>
Factor<KEY>::Factor(const ConditionalType& c) : FactorOrdered<KEY>::FactorOrdered(const ConditionalType& c) :
keys_(c.keys()) { keys_(c.keys()) {
// assert(c.nrFrontals() == 1); // assert(c.nrFrontals() == 1);
assertInvariants(); assertInvariants();
@ -42,7 +42,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<typename KEY> template<typename KEY>
void Factor<KEY>::assertInvariants() const { void FactorOrdered<KEY>::assertInvariants() const {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
// Check that keys are all unique // Check that keys are all unique
std::multiset<KeyType> nonunique(keys_.begin(), keys_.end()); std::multiset<KeyType> nonunique(keys_.begin(), keys_.end());
@ -56,14 +56,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<typename KEY> template<typename KEY>
void Factor<KEY>::print(const std::string& s, void FactorOrdered<KEY>::print(const std::string& s,
const IndexFormatter& formatter) const { const IndexFormatter& formatter) const {
printKeys(s,formatter); printKeys(s,formatter);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<typename KEY> template<typename KEY>
void Factor<KEY>::printKeys(const std::string& s, const IndexFormatter& formatter) const { void FactorOrdered<KEY>::printKeys(const std::string& s, const IndexFormatter& formatter) const {
std::cout << s << " "; std::cout << s << " ";
BOOST_FOREACH(KEY key, keys_) std::cout << " " << formatter(key); BOOST_FOREACH(KEY key, keys_) std::cout << " " << formatter(key);
std::cout << std::endl; std::cout << std::endl;
@ -71,7 +71,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<typename KEY> template<typename KEY>
bool Factor<KEY>::equals(const This& other, double tol) const { bool FactorOrdered<KEY>::equals(const This& other, double tol) const {
return keys_ == other.keys_; return keys_ == other.keys_;
} }
@ -79,7 +79,7 @@ namespace gtsam {
#ifdef TRACK_ELIMINATE #ifdef TRACK_ELIMINATE
template<typename KEY> template<typename KEY>
template<class COND> template<class COND>
typename COND::shared_ptr Factor<KEY>::eliminateFirst() { typename COND::shared_ptr FactorOrdered<KEY>::eliminateFirst() {
assert(!keys_.empty()); assert(!keys_.empty());
assertInvariants(); assertInvariants();
KEY eliminated = keys_.front(); KEY eliminated = keys_.front();
@ -91,10 +91,10 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<typename KEY> template<typename KEY>
template<class COND> template<class COND>
typename BayesNet<COND>::shared_ptr Factor<KEY>::eliminate(size_t nrFrontals) { typename BayesNetOrdered<COND>::shared_ptr FactorOrdered<KEY>::eliminate(size_t nrFrontals) {
assert(keys_.size() >= nrFrontals); assert(keys_.size() >= nrFrontals);
assertInvariants(); assertInvariants();
typename BayesNet<COND>::shared_ptr fragment(new BayesNet<COND> ()); typename BayesNetOrdered<COND>::shared_ptr fragment(new BayesNetOrdered<COND> ());
const_iterator it = this->begin(); const_iterator it = this->begin();
for (KEY n = 0; n < nrFrontals; ++n, ++it) for (KEY n = 0; n < nrFrontals; ++n, ++it)
fragment->push_back(COND::FromRange(it, const_iterator(this->end()), 1)); fragment->push_back(COND::FromRange(it, const_iterator(this->end()), 1));

232
gtsam/inference/FactorOrdered.h Normal file
View File

@ -0,0 +1,232 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Factor.h
* @brief The base class for all factors
* @author Kai Ni
* @author Frank Dellaert
* @author Richard Roberts
*/
// \callgraph
#pragma once
#include <set>
#include <vector>
#include <boost/serialization/nvp.hpp>
#include <boost/foreach.hpp>
#include <boost/function/function1.hpp>
#include <boost/lexical_cast.hpp>
#include <gtsam/global_includes.h>
#include <gtsam/base/FastMap.h>
namespace gtsam {
template<class KEY> class ConditionalOrdered;
/**
* This is the base class for all factor types. It is templated on a KEY type,
* which will be the type used to label variables. Key types currently in use
* in gtsam are Index with symbolic (IndexFactor, SymbolicFactorGraph) and
* Gaussian factors (GaussianFactor, JacobianFactor, HessianFactor, GaussianFactorGraph),
* and Key with nonlinear factors (NonlinearFactor, NonlinearFactorGraph).
* though currently only IndexFactor and IndexConditional derive from this
* class, using Index keys. This class does not store any data other than its
* keys. Derived classes store data such as matrices and probability tables.
*
* Note that derived classes *must* redefine the ConditionalType and shared_ptr
* typedefs to refer to the associated conditional and shared_ptr types of the
* derived class. See IndexFactor, JacobianFactor, etc. for examples.
*
* This class is \b not virtual for performance reasons - derived symbolic classes,
* IndexFactor and IndexConditional, need to be created and destroyed quickly
* during symbolic elimination. GaussianFactor and NonlinearFactor are virtual.
* \nosubgrouping
*/
template<typename KEY>
class FactorOrdered {
public:
typedef KEY KeyType; ///< The KEY template parameter
typedef FactorOrdered<KeyType> This; ///< This class
/**
* Typedef to the conditional type obtained by eliminating this factor,
* derived classes must redefine this.
*/
typedef ConditionalOrdered<KeyType> ConditionalType;
/// A shared_ptr to this class, derived classes must redefine this.
typedef boost::shared_ptr<FactorOrdered> shared_ptr;
/// Iterator over keys
typedef typename std::vector<KeyType>::iterator iterator;
/// Const iterator over keys
typedef typename std::vector<KeyType>::const_iterator const_iterator;
protected:
/// The keys involved in this factor
std::vector<KeyType> keys_;
public:
/// @name Standard Constructors
/// @{
/** Copy constructor */
FactorOrdered(const This& f);
/** Construct from conditional, calls ConditionalType::toFactor() */
FactorOrdered(const ConditionalType& c);
/** Default constructor for I/O */
FactorOrdered() {}
/** Construct unary factor */
FactorOrdered(KeyType key) : keys_(1) {
keys_[0] = key; assertInvariants(); }
/** Construct binary factor */
FactorOrdered(KeyType key1, KeyType key2) : keys_(2) {
keys_[0] = key1; keys_[1] = key2; assertInvariants(); }
/** Construct ternary factor */
FactorOrdered(KeyType key1, KeyType key2, KeyType key3) : keys_(3) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); }
/** Construct 4-way factor */
FactorOrdered(KeyType key1, KeyType key2, KeyType key3, KeyType key4) : keys_(4) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); }
/** Construct 5-way factor */
FactorOrdered(KeyType key1, KeyType key2, KeyType key3, KeyType key4, KeyType key5) : keys_(5) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; assertInvariants(); }
/** Construct 6-way factor */
FactorOrdered(KeyType key1, KeyType key2, KeyType key3, KeyType key4, KeyType key5, KeyType key6) : keys_(6) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; keys_[5] = key6; assertInvariants(); }
/// @}
/// @name Advanced Constructors
/// @{
/** Construct n-way factor */
FactorOrdered(const std::set<KeyType>& keys) {
BOOST_FOREACH(const KeyType& key, keys) keys_.push_back(key);
assertInvariants();
}
/** Construct n-way factor */
FactorOrdered(const std::vector<KeyType>& keys) : keys_(keys) {
assertInvariants();
}
/** Constructor from a collection of keys */
template<class KEYITERATOR> FactorOrdered(KEYITERATOR beginKey, KEYITERATOR endKey) :
keys_(beginKey, endKey) { assertInvariants(); }
/// @}
#ifdef TRACK_ELIMINATE
/**
* eliminate the first variable involved in this factor
* @return a conditional on the eliminated variable
*/
template<class CONDITIONAL>
typename CONDITIONAL::shared_ptr eliminateFirst();
/**
* eliminate the first nrFrontals frontal variables.
*/
template<class CONDITIONAL>
typename BayesNetOrdered<CONDITIONAL>::shared_ptr eliminate(size_t nrFrontals = 1);
#endif
/// @name Standard Interface
/// @{
/// First key
KeyType front() const { return keys_.front(); }
/// Last key
KeyType back() const { return keys_.back(); }
/// find
const_iterator find(KeyType key) const { return std::find(begin(), end(), key); }
/// Access the factor's involved variable keys
const std::vector<KeyType>& keys() const { return keys_; }
/** iterators */
const_iterator begin() const { return keys_.begin(); } ///TODO: comment
const_iterator end() const { return keys_.end(); } ///TODO: comment
/**
* @return the number of variables involved in this factor
*/
size_t size() const { return keys_.size(); }
/// @}
/// @name Testable
/// @{
/// print
void print(const std::string& s = "Factor",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/// print only keys
void printKeys(const std::string& s = "Factor",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/// check equality
bool equals(const This& other, double tol = 1e-9) const;
/// @}
/// @name Advanced Interface
/// @{
/**
* @return keys involved in this factor
*/
std::vector<KeyType>& keys() { return keys_; }
/** mutable iterators */
iterator begin() { return keys_.begin(); } ///TODO: comment
iterator end() { return keys_.end(); } ///TODO: comment
protected:
friend class JacobianFactorOrdered;
friend class HessianFactorOrdered;
/// Internal consistency check that is run frequently when in debug mode.
/// If NDEBUG is defined, this is empty and optimized out.
void assertInvariants() const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(keys_);
}
/// @}
};
}
#include <gtsam/inference/FactorOrdered-inl.h>

171
gtsam/inference/FactorUnordered.h
View File

@ -1,171 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Factor.h
* @brief The base class for all factors
* @author Kai Ni
* @author Frank Dellaert
* @author Richard Roberts
*/
// \callgraph
#pragma once
#include <vector>
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/types.h>
#include <gtsam/inference/Key.h>
namespace gtsam {
/**
* This is the base class for all factor types. It is templated on a KEY type,
* which will be the type used to label variables. Key types currently in use
* in gtsam are Index with symbolic (IndexFactor, SymbolicFactorGraph) and
* Gaussian factors (GaussianFactor, JacobianFactor, HessianFactor, GaussianFactorGraph),
* and Key with nonlinear factors (NonlinearFactor, NonlinearFactorGraph).
* though currently only IndexFactor and IndexConditional derive from this
* class, using Index keys. This class does not store any data other than its
* keys. Derived classes store data such as matrices and probability tables.
*
* Note that derived classes *must* redefine the ConditionalType and shared_ptr
* typedefs to refer to the associated conditional and shared_ptr types of the
* derived class. See IndexFactor, JacobianFactor, etc. for examples.
*
* This class is \b not virtual for performance reasons - derived symbolic classes,
* IndexFactor and IndexConditional, need to be created and destroyed quickly
* during symbolic elimination. GaussianFactor and NonlinearFactor are virtual.
* \nosubgrouping
*/
class GTSAM_EXPORT FactorUnordered
{
private:
// These typedefs are private because they must be overridden in derived classes.
typedef FactorUnordered This; ///< This class
typedef boost::shared_ptr<FactorUnordered> shared_ptr; ///< A shared_ptr to this class.
public:
/// Iterator over keys
typedef std::vector<Key>::iterator iterator;
/// Const iterator over keys
typedef std::vector<Key>::const_iterator const_iterator;
protected:
/// The keys involved in this factor
std::vector<Key> keys_;
/// @name Standard Constructors
/// @{
/** Default constructor for I/O */
FactorUnordered() {}
/** Construct factor from container of keys. This constructor is used internally from derived factor
* constructors, either from a container of keys or from a boost::assign::list_of. */
template<typename CONTAINER>
FactorUnordered(const CONTAINER& keys) : keys_(keys.begin(), keys.end()) {}
/** Construct factor from iterator keys. This constructor may be used internally from derived
* factor constructors, although our code currently does not use this. */
template<typename ITERATOR>
FactorUnordered(ITERATOR first, ITERATOR last) : keys_(first, last) {}
/** Construct factor from container of keys. This is called internally from derived factor static
* factor methods, as a workaround for not being able to call the protected constructors above. */
template<typename CONTAINER>
static FactorUnordered FromKeys(const CONTAINER& keys) {
return FactorUnordered(keys.begin(), keys.end()); }
/** Construct factor from iterator keys. This is called internally from derived factor static
* factor methods, as a workaround for not being able to call the protected constructors above. */
template<typename ITERATOR>
static FactorUnordered FromIterators(ITERATOR first, ITERATOR last) {
return FactorUnordered(first, last); }
/// @}
public:
/// @name Standard Interface
/// @{
/// First key
Key front() const { return keys_.front(); }
/// Last key
Key back() const { return keys_.back(); }
/// find
const_iterator find(Key key) const { return std::find(begin(), end(), key); }
/// Access the factor's involved variable keys
const std::vector<Key>& keys() const { return keys_; }
/** Iterator at beginning of involved variable keys */
const_iterator begin() const { return keys_.begin(); }
/** Iterator at end of involved variable keys */
const_iterator end() const { return keys_.end(); }
/**
* @return the number of variables involved in this factor
*/
size_t size() const { return keys_.size(); }
/// @}
/// @name Testable
/// @{
/// print
void print(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// print only keys
void printKeys(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const;
protected:
/// check equality
bool equals(const This& other, double tol = 1e-9) const;
/// @}
public:
/// @name Advanced Interface
/// @{
/** @return keys involved in this factor */
std::vector<Key>& keys() { return keys_; }
/** Iterator at beginning of involved variable keys */
iterator begin() { return keys_.begin(); }
/** Iterator at end of involved variable keys */
iterator end() { return keys_.end(); }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(keys_);
}
/// @}
};
}

22
gtsam/inference/GenericMultifrontalSolver-inl.h
View File

@ -17,17 +17,17 @@
#pragma once #pragma once
#include <gtsam/inference/Factor-inl.h> #include <gtsam/inference/FactorOrdered-inl.h>
#include <gtsam/inference/JunctionTree.h> #include <gtsam/inference/JunctionTreeOrdered.h>
#include <gtsam/inference/BayesNet-inl.h> #include <gtsam/inference/BayesNetOrdered-inl.h>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class F, class JT> template<class F, class JT>
GenericMultifrontalSolver<F, JT>::GenericMultifrontalSolver( GenericMultifrontalSolver<F, JT>::GenericMultifrontalSolver(
const FactorGraph<F>& graph) : const FactorGraphOrdered<F>& graph) :
structure_(new VariableIndex(graph)), junctionTree_( structure_(new VariableIndexOrdered(graph)), junctionTree_(
new JT(graph, *structure_)) { new JT(graph, *structure_)) {
} }
@ -35,7 +35,7 @@ namespace gtsam {
template<class F, class JT> template<class F, class JT>
GenericMultifrontalSolver<F, JT>::GenericMultifrontalSolver( GenericMultifrontalSolver<F, JT>::GenericMultifrontalSolver(
const sharedGraph& graph, const sharedGraph& graph,
const VariableIndex::shared_ptr& variableIndex) : const VariableIndexOrdered::shared_ptr& variableIndex) :
structure_(variableIndex), junctionTree_(new JT(*graph, *structure_)) { structure_(variableIndex), junctionTree_(new JT(*graph, *structure_)) {
} }
@ -63,16 +63,16 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR, class JUNCTIONTREE> template<class FACTOR, class JUNCTIONTREE>
typename BayesTree<typename FACTOR::ConditionalType>::shared_ptr typename BayesTreeOrdered<typename FACTOR::ConditionalType>::shared_ptr
GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::eliminate(Eliminate function) const { GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::eliminate(Eliminate function) const {
// eliminate junction tree, returns pointer to root // eliminate junction tree, returns pointer to root
typename BayesTree<typename FACTOR::ConditionalType>::sharedClique typename BayesTreeOrdered<typename FACTOR::ConditionalType>::sharedClique
root = junctionTree_->eliminate(function); root = junctionTree_->eliminate(function);
// create an empty Bayes tree and insert root clique // create an empty Bayes tree and insert root clique
typename BayesTree<typename FACTOR::ConditionalType>::shared_ptr typename BayesTreeOrdered<typename FACTOR::ConditionalType>::shared_ptr
bayesTree(new BayesTree<typename FACTOR::ConditionalType>); bayesTree(new BayesTreeOrdered<typename FACTOR::ConditionalType>);
bayesTree->insert(root); bayesTree->insert(root);
// return the Bayes tree // return the Bayes tree
@ -81,7 +81,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class F, class JT> template<class F, class JT>
typename FactorGraph<F>::shared_ptr GenericMultifrontalSolver<F, JT>::jointFactorGraph( typename FactorGraphOrdered<F>::shared_ptr GenericMultifrontalSolver<F, JT>::jointFactorGraph(
const std::vector<Index>& js, Eliminate function) const { const std::vector<Index>& js, Eliminate function) const {
// FIXME: joint for arbitrary sets of variables not present // FIXME: joint for arbitrary sets of variables not present

18
gtsam/inference/GenericMultifrontalSolver.h
View File

@ -19,8 +19,8 @@
#include <utility> #include <utility>
#include <gtsam/inference/JunctionTree.h> #include <gtsam/inference/JunctionTreeOrdered.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraphOrdered.h>
namespace gtsam { namespace gtsam {
@ -44,15 +44,15 @@ namespace gtsam {
protected: protected:
/// Column structure of the factor graph /// Column structure of the factor graph
VariableIndex::shared_ptr structure_; VariableIndexOrdered::shared_ptr structure_;
/// Junction tree that performs elimination. /// Junction tree that performs elimination.
typename JUNCTIONTREE::shared_ptr junctionTree_; typename JUNCTIONTREE::shared_ptr junctionTree_;
public: public:
typedef typename FactorGraph<FACTOR>::shared_ptr sharedGraph; typedef typename FactorGraphOrdered<FACTOR>::shared_ptr sharedGraph;
typedef typename FactorGraph<FACTOR>::Eliminate Eliminate; typedef typename FactorGraphOrdered<FACTOR>::Eliminate Eliminate;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -62,7 +62,7 @@ namespace gtsam {
* tree, which does the symbolic elimination, identifies the cliques, * tree, which does the symbolic elimination, identifies the cliques,
* and distributes all the factors to the right cliques. * and distributes all the factors to the right cliques.
*/ */
GenericMultifrontalSolver(const FactorGraph<FACTOR>& factorGraph); GenericMultifrontalSolver(const FactorGraphOrdered<FACTOR>& factorGraph);
/** /**
* Construct the solver with a shared pointer to a factor graph and to a * Construct the solver with a shared pointer to a factor graph and to a
@ -70,7 +70,7 @@ namespace gtsam {
* is the fastest. * is the fastest.
*/ */
GenericMultifrontalSolver(const sharedGraph& factorGraph, GenericMultifrontalSolver(const sharedGraph& factorGraph,
const VariableIndex::shared_ptr& variableIndex); const VariableIndexOrdered::shared_ptr& variableIndex);
/// @} /// @}
/// @name Testable /// @name Testable
@ -97,7 +97,7 @@ namespace gtsam {
* Eliminate the factor graph sequentially. Uses a column elimination tree * Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate. * to recursively eliminate.
*/ */
typename BayesTree<typename FACTOR::ConditionalType>::shared_ptr typename BayesTreeOrdered<typename FACTOR::ConditionalType>::shared_ptr
eliminate(Eliminate function) const; eliminate(Eliminate function) const;
/** /**
@ -105,7 +105,7 @@ namespace gtsam {
* all of the other variables. This function returns the result as a factor * all of the other variables. This function returns the result as a factor
* graph. * graph.
*/ */
typename FactorGraph<FACTOR>::shared_ptr jointFactorGraph( typename FactorGraphOrdered<FACTOR>::shared_ptr jointFactorGraph(
const std::vector<Index>& js, Eliminate function) const; const std::vector<Index>& js, Eliminate function) const;
/** /**

32
gtsam/inference/GenericSequentialSolver-inl.h
View File

@ -18,11 +18,11 @@
#pragma once #pragma once
#include <gtsam/inference/Factor.h> #include <gtsam/inference/FactorOrdered.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraphOrdered.h>
#include <gtsam/inference/EliminationTree.h> #include <gtsam/inference/EliminationTreeOrdered.h>
#include <gtsam/inference/BayesNet.h> #include <gtsam/inference/BayesNetOrdered.h>
#include <gtsam/inference/inference.h> #include <gtsam/inference/inferenceOrdered.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
@ -30,24 +30,24 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph) { GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const FactorGraphOrdered<FACTOR>& factorGraph) {
gttic(GenericSequentialSolver_constructor1); gttic(GenericSequentialSolver_constructor1);
assert(factorGraph.size()); assert(factorGraph.size());
factors_.reset(new FactorGraph<FACTOR>(factorGraph)); factors_.reset(new FactorGraphOrdered<FACTOR>(factorGraph));
structure_.reset(new VariableIndex(factorGraph)); structure_.reset(new VariableIndexOrdered(factorGraph));
eliminationTree_ = EliminationTree<FACTOR>::Create(*factors_, *structure_); eliminationTree_ = EliminationTreeOrdered<FACTOR>::Create(*factors_, *structure_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver( GenericSequentialSolver<FACTOR>::GenericSequentialSolver(
const sharedFactorGraph& factorGraph, const sharedFactorGraph& factorGraph,
const boost::shared_ptr<VariableIndex>& variableIndex) const boost::shared_ptr<VariableIndexOrdered>& variableIndex)
{ {
gttic(GenericSequentialSolver_constructor2); gttic(GenericSequentialSolver_constructor2);
factors_ = factorGraph; factors_ = factorGraph;
structure_ = variableIndex; structure_ = variableIndex;
eliminationTree_ = EliminationTree<FACTOR>::Create(*factors_, *structure_); eliminationTree_ = EliminationTreeOrdered<FACTOR>::Create(*factors_, *structure_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -79,7 +79,7 @@ namespace gtsam {
// problems there may not be enough memory to store two copies. // problems there may not be enough memory to store two copies.
eliminationTree_.reset(); eliminationTree_.reset();
factors_ = factorGraph; factors_ = factorGraph;
eliminationTree_ = EliminationTree<FACTOR>::Create(*factors_, *structure_); eliminationTree_ = EliminationTreeOrdered<FACTOR>::Create(*factors_, *structure_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -107,7 +107,7 @@ namespace gtsam {
factor->permuteWithInverse(*permutationInverse); factor->permuteWithInverse(*permutationInverse);
// Eliminate using elimination tree provided // Eliminate using elimination tree provided
typename EliminationTree<FACTOR>::shared_ptr etree = EliminationTree<FACTOR>::Create(*factors_); typename EliminationTreeOrdered<FACTOR>::shared_ptr etree = EliminationTreeOrdered<FACTOR>::Create(*factors_);
sharedBayesNet bayesNet; sharedBayesNet bayesNet;
if(nrToEliminate) if(nrToEliminate)
bayesNet = etree->eliminatePartial(function, *nrToEliminate); bayesNet = etree->eliminatePartial(function, *nrToEliminate);
@ -175,15 +175,15 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
typename FactorGraph<FACTOR>::shared_ptr // typename FactorGraphOrdered<FACTOR>::shared_ptr //
GenericSequentialSolver<FACTOR>::jointFactorGraph( GenericSequentialSolver<FACTOR>::jointFactorGraph(
const std::vector<Index>& js, Eliminate function) const const std::vector<Index>& js, Eliminate function) const
{ {
gttic(GenericSequentialSolver_jointFactorGraph); gttic(GenericSequentialSolver_jointFactorGraph);
// Eliminate all variables // Eliminate all variables
typename BayesNet<Conditional>::shared_ptr bayesNet = jointBayesNet(js, function); typename BayesNetOrdered<Conditional>::shared_ptr bayesNet = jointBayesNet(js, function);
return boost::make_shared<FactorGraph<FACTOR> >(*bayesNet); return boost::make_shared<FactorGraphOrdered<FACTOR> >(*bayesNet);
} }
/* ************************************************************************* */ /* ************************************************************************* */

26
gtsam/inference/GenericSequentialSolver.h
View File

@ -28,17 +28,17 @@
#include <vector> #include <vector>
namespace gtsam { namespace gtsam {
class VariableIndex; class VariableIndexOrdered;
class Permutation; class Permutation;
} }
namespace gtsam { namespace gtsam {
template<class FACTOR> class EliminationTree; template<class FACTOR> class EliminationTreeOrdered;
} }
namespace gtsam { namespace gtsam {
template<class FACTOR> class FactorGraph; template<class FACTOR> class FactorGraphOrdered;
} }
namespace gtsam { namespace gtsam {
template<class CONDITIONAL> class BayesNet; template<class CONDITIONAL> class BayesNetOrdered;
} }
namespace gtsam { namespace gtsam {
@ -62,13 +62,13 @@ namespace gtsam {
protected: protected:
typedef boost::shared_ptr<FactorGraph<FACTOR> > sharedFactorGraph; typedef boost::shared_ptr<FactorGraphOrdered<FACTOR> > sharedFactorGraph;
typedef typename FACTOR::ConditionalType Conditional; typedef typename FACTOR::ConditionalType Conditional;
typedef typename boost::shared_ptr<Conditional> sharedConditional; typedef typename boost::shared_ptr<Conditional> sharedConditional;
typedef typename boost::shared_ptr<BayesNet<Conditional> > sharedBayesNet; typedef typename boost::shared_ptr<BayesNetOrdered<Conditional> > sharedBayesNet;
typedef std::pair<boost::shared_ptr<Conditional>, boost::shared_ptr<FACTOR> > EliminationResult; typedef std::pair<boost::shared_ptr<Conditional>, boost::shared_ptr<FACTOR> > EliminationResult;
typedef boost::function< typedef boost::function<
EliminationResult(const FactorGraph<FACTOR>&, size_t)> Eliminate; EliminationResult(const FactorGraphOrdered<FACTOR>&, size_t)> Eliminate;
/** Store the original factors for computing marginals /** Store the original factors for computing marginals
* TODO Frank says: really? Marginals should be computed from result. * TODO Frank says: really? Marginals should be computed from result.
@ -76,14 +76,14 @@ namespace gtsam {
sharedFactorGraph factors_; sharedFactorGraph factors_;
/** Store column structure of the factor graph. Why? */ /** Store column structure of the factor graph. Why? */
boost::shared_ptr<VariableIndex> structure_; boost::shared_ptr<VariableIndexOrdered> structure_;
/** Elimination tree that performs elimination */ /** Elimination tree that performs elimination */
boost::shared_ptr<EliminationTree<FACTOR> > eliminationTree_; boost::shared_ptr<EliminationTreeOrdered<FACTOR> > eliminationTree_;
/** concept checks */ /** concept checks */
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
// GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree) // GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTreeOrdered)
/** /**
* Eliminate in a different order, given a permutation * Eliminate in a different order, given a permutation
@ -101,7 +101,7 @@ namespace gtsam {
* Construct the solver for a factor graph. This builds the elimination * Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the work of elimination. * tree, which already does some of the work of elimination.
*/ */
GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph); GenericSequentialSolver(const FactorGraphOrdered<FACTOR>& factorGraph);
/** /**
* Construct the solver with a shared pointer to a factor graph and to a * Construct the solver with a shared pointer to a factor graph and to a
@ -109,7 +109,7 @@ namespace gtsam {
* is the fastest. * is the fastest.
*/ */
GenericSequentialSolver(const sharedFactorGraph& factorGraph, GenericSequentialSolver(const sharedFactorGraph& factorGraph,
const boost::shared_ptr<VariableIndex>& variableIndex); const boost::shared_ptr<VariableIndexOrdered>& variableIndex);
/// @} /// @}
/// @name Testable /// @name Testable
@ -158,7 +158,7 @@ namespace gtsam {
* Compute the marginal joint over a set of variables, by integrating out * Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. Returns the result as a factor graph. * all of the other variables. Returns the result as a factor graph.
*/ */
typename FactorGraph<FACTOR>::shared_ptr typename FactorGraphOrdered<FACTOR>::shared_ptr
jointFactorGraph(const std::vector<Index>& js, Eliminate function) const; jointFactorGraph(const std::vector<Index>& js, Eliminate function) const;
/** /**

16
gtsam/inference/ISAM-inl.h → gtsam/inference/ISAMOrdered-inl.h
View File

@ -20,24 +20,24 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/ConditionalOrdered.h>
#include <gtsam/inference/BayesTree.h> #include <gtsam/inference/BayesTreeOrdered.h>
#include <gtsam/inference/GenericMultifrontalSolver.h> #include <gtsam/inference/GenericMultifrontalSolver.h>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
ISAM<CONDITIONAL>::ISAM() : BayesTree<CONDITIONAL>() {} ISAMOrdered<CONDITIONAL>::ISAMOrdered() : BayesTreeOrdered<CONDITIONAL>() {}
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
template<class FG> void ISAM<CONDITIONAL>::update_internal( template<class FG> void ISAMOrdered<CONDITIONAL>::update_internal(
const FG& newFactors, Cliques& orphans, typename FG::Eliminate function) { const FG& newFactors, Cliques& orphans, typename FG::Eliminate function) {
// Remove the contaminated part of the Bayes tree // Remove the contaminated part of the Bayes tree
// Throw exception if disconnected // Throw exception if disconnected
BayesNet<CONDITIONAL> bn; BayesNetOrdered<CONDITIONAL> bn;
if (!this->empty()) { if (!this->empty()) {
this->removeTop(newFactors.keys(), bn, orphans); this->removeTop(newFactors.keys(), bn, orphans);
if (bn.empty()) if (bn.empty())
@ -50,8 +50,8 @@ namespace gtsam {
factors.push_back(newFactors); factors.push_back(newFactors);
// eliminate into a Bayes net // eliminate into a Bayes net
GenericMultifrontalSolver<typename CONDITIONAL::FactorType, JunctionTree<FG> > solver(factors); GenericMultifrontalSolver<typename CONDITIONAL::FactorType, JunctionTreeOrdered<FG> > solver(factors);
boost::shared_ptr<BayesTree<CONDITIONAL> > bayesTree; boost::shared_ptr<BayesTreeOrdered<CONDITIONAL> > bayesTree;
bayesTree = solver.eliminate(function); bayesTree = solver.eliminate(function);
this->root_ = bayesTree->root(); this->root_ = bayesTree->root();
this->nodes_ = bayesTree->nodes(); this->nodes_ = bayesTree->nodes();
@ -64,7 +64,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
template<class FG> template<class FG>
void ISAM<CONDITIONAL>::update(const FG& newFactors, void ISAMOrdered<CONDITIONAL>::update(const FG& newFactors,
typename FG::Eliminate function) { typename FG::Eliminate function) {
Cliques orphans; Cliques orphans;
this->update_internal(newFactors, orphans, function); this->update_internal(newFactors, orphans, function);

16
gtsam/inference/ISAM.h → gtsam/inference/ISAMOrdered.h
View File

@ -18,7 +18,7 @@
// \callgraph // \callgraph
#pragma once #pragma once
#include <gtsam/inference/BayesTree.h> #include <gtsam/inference/BayesTreeOrdered.h>
namespace gtsam { namespace gtsam {
@ -28,11 +28,11 @@ namespace gtsam {
* \nosubgrouping * \nosubgrouping
*/ */
template<class CONDITIONAL> template<class CONDITIONAL>
class ISAM: public BayesTree<CONDITIONAL> { class ISAMOrdered: public BayesTreeOrdered<CONDITIONAL> {
private: private:
typedef BayesTree<CONDITIONAL> Base; typedef BayesTreeOrdered<CONDITIONAL> Base;
public: public:
@ -40,10 +40,10 @@ namespace gtsam {
/// @{ /// @{
/** Create an empty Bayes Tree */ /** Create an empty Bayes Tree */
ISAM(); ISAMOrdered();
/** Copy constructor */ /** Copy constructor */
ISAM(const Base& bayesTree) : ISAMOrdered(const Base& bayesTree) :
Base(bayesTree) { Base(bayesTree) {
} }
@ -59,8 +59,8 @@ namespace gtsam {
template<class FG> template<class FG>
void update(const FG& newFactors, typename FG::Eliminate function); void update(const FG& newFactors, typename FG::Eliminate function);
typedef typename BayesTree<CONDITIONAL>::sharedClique sharedClique; ///<TODO: comment typedef typename BayesTreeOrdered<CONDITIONAL>::sharedClique sharedClique; ///<TODO: comment
typedef typename BayesTree<CONDITIONAL>::Cliques Cliques; ///<TODO: comment typedef typename BayesTreeOrdered<CONDITIONAL>::Cliques Cliques; ///<TODO: comment
/** update_internal provides access to list of orphans for drawing purposes */ /** update_internal provides access to list of orphans for drawing purposes */
template<class FG> template<class FG>
@ -73,4 +73,4 @@ namespace gtsam {
}/// namespace gtsam }/// namespace gtsam
#include <gtsam/inference/ISAM-inl.h> #include <gtsam/inference/ISAMOrdered-inl.h>

8
gtsam/inference/IndexConditional.cpp → gtsam/inference/IndexConditionalOrdered.cpp
View File

@ -16,7 +16,7 @@
*/ */
#include <gtsam/base/FastSet.h> #include <gtsam/base/FastSet.h>
#include <gtsam/inference/IndexConditional.h> #include <gtsam/inference/IndexConditionalOrdered.h>
#ifdef __GNUC__ #ifdef __GNUC__
#pragma GCC diagnostic push #pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable" #pragma GCC diagnostic ignored "-Wunused-variable"
@ -32,13 +32,13 @@ namespace gtsam {
using namespace boost::lambda; using namespace boost::lambda;
/* ************************************************************************* */ /* ************************************************************************* */
void IndexConditional::assertInvariants() const { void IndexConditionalOrdered::assertInvariants() const {
// Checks for uniqueness of keys // Checks for uniqueness of keys
Base::assertInvariants(); Base::assertInvariants();
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool IndexConditional::reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) { bool IndexConditionalOrdered::reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
BOOST_FOREACH(KeyType key, frontals()) { assert(inverseReduction.find(key) == inverseReduction.end()); } BOOST_FOREACH(KeyType key, frontals()) { assert(inverseReduction.find(key) == inverseReduction.end()); }
#endif #endif
@ -55,7 +55,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) { void IndexConditionalOrdered::permuteWithInverse(const Permutation& inversePermutation) {
BOOST_FOREACH(Index& key, keys()) BOOST_FOREACH(Index& key, keys())
key = inversePermutation[key]; key = inversePermutation[key];
assertInvariants(); assertInvariants();

38
gtsam/inference/IndexConditional.h → gtsam/inference/IndexConditionalOrdered.h
View File

@ -18,9 +18,9 @@
#pragma once #pragma once
#include <gtsam/global_includes.h> #include <gtsam/global_includes.h>
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/ConditionalOrdered.h>
#include <gtsam/inference/IndexFactor.h> #include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/Permutation.h> #include <gtsam/inference/PermutationOrdered.h>
namespace gtsam { namespace gtsam {
@ -33,7 +33,7 @@ namespace gtsam {
* unsigned integer. * unsigned integer.
* \nosubgrouping * \nosubgrouping
*/ */
class IndexConditional : public Conditional<Index> { class IndexConditionalOrdered : public ConditionalOrdered<Index> {
protected: protected:
@ -42,46 +42,46 @@ namespace gtsam {
public: public:
typedef IndexConditional This; typedef IndexConditionalOrdered This;
typedef Conditional<Index> Base; typedef ConditionalOrdered<Index> Base;
typedef IndexFactor FactorType; typedef IndexFactorOrdered FactorType;
typedef boost::shared_ptr<IndexConditional> shared_ptr; typedef boost::shared_ptr<IndexConditionalOrdered> shared_ptr;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
/** Empty Constructor to make serialization possible */ /** Empty Constructor to make serialization possible */
IndexConditional() { assertInvariants(); } IndexConditionalOrdered() { assertInvariants(); }
/** No parents */ /** No parents */
IndexConditional(Index j) : Base(j) { assertInvariants(); } IndexConditionalOrdered(Index j) : Base(j) { assertInvariants(); }
/** Single parent */ /** Single parent */
IndexConditional(Index j, Index parent) : Base(j, parent) { assertInvariants(); } IndexConditionalOrdered(Index j, Index parent) : Base(j, parent) { assertInvariants(); }
/** Two parents */ /** Two parents */
IndexConditional(Index j, Index parent1, Index parent2) : Base(j, parent1, parent2) { assertInvariants(); } IndexConditionalOrdered(Index j, Index parent1, Index parent2) : Base(j, parent1, parent2) { assertInvariants(); }
/** Three parents */ /** Three parents */
IndexConditional(Index j, Index parent1, Index parent2, Index parent3) : Base(j, parent1, parent2, parent3) { assertInvariants(); } IndexConditionalOrdered(Index j, Index parent1, Index parent2, Index parent3) : Base(j, parent1, parent2, parent3) { assertInvariants(); }
/// @} /// @}
/// @name Advanced Constructors /// @name Advanced Constructors
/// @{ /// @{
/** Constructor from a frontal variable and a vector of parents */ /** Constructor from a frontal variable and a vector of parents */
IndexConditional(Index j, const std::vector<Index>& parents) : Base(j, parents) { IndexConditionalOrdered(Index j, const std::vector<Index>& parents) : Base(j, parents) {
assertInvariants(); assertInvariants();
} }
/** Constructor from keys and nr of frontal variables */ /** Constructor from keys and nr of frontal variables */
IndexConditional(const std::vector<Index>& keys, size_t nrFrontals) : IndexConditionalOrdered(const std::vector<Index>& keys, size_t nrFrontals) :
Base(keys, nrFrontals) { Base(keys, nrFrontals) {
assertInvariants(); assertInvariants();
} }
/** Constructor from keys and nr of frontal variables */ /** Constructor from keys and nr of frontal variables */
IndexConditional(const std::list<Index>& keys, size_t nrFrontals) : IndexConditionalOrdered(const std::list<Index>& keys, size_t nrFrontals) :
Base(keys, nrFrontals) { Base(keys, nrFrontals) {
assertInvariants(); assertInvariants();
} }
@ -93,15 +93,15 @@ namespace gtsam {
/** Named constructor directly returning a shared pointer */ /** Named constructor directly returning a shared pointer */
template<class KEYS> template<class KEYS>
static shared_ptr FromKeys(const KEYS& keys, size_t nrFrontals) { static shared_ptr FromKeys(const KEYS& keys, size_t nrFrontals) {
shared_ptr conditional(new IndexConditional()); shared_ptr conditional(new IndexConditionalOrdered());
conditional->keys_.assign(keys.begin(), keys.end()); conditional->keys_.assign(keys.begin(), keys.end());
conditional->nrFrontals_ = nrFrontals; conditional->nrFrontals_ = nrFrontals;
return conditional; return conditional;
} }
/** Convert to a factor */ /** Convert to a factor */
IndexFactor::shared_ptr toFactor() const { IndexFactorOrdered::shared_ptr toFactor() const {
return IndexFactor::shared_ptr(new IndexFactor(*this)); return IndexFactorOrdered::shared_ptr(new IndexFactorOrdered(*this));
} }
/// @} /// @}

26
gtsam/inference/IndexFactor.cpp → gtsam/inference/IndexFactorOrdered.cpp
View File

@ -15,53 +15,53 @@
* @date Oct 17, 2010 * @date Oct 17, 2010
*/ */
#include <gtsam/inference/IndexFactor.h> #include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/Factor.h> #include <gtsam/inference/FactorOrdered.h>
#include <gtsam/inference/IndexConditional.h> #include <gtsam/inference/IndexConditionalOrdered.h>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void IndexFactor::assertInvariants() const { void IndexFactorOrdered::assertInvariants() const {
Base::assertInvariants(); Base::assertInvariants();
} }
/* ************************************************************************* */ /* ************************************************************************* */
IndexFactor::IndexFactor(const IndexConditional& c) : IndexFactorOrdered::IndexFactorOrdered(const IndexConditionalOrdered& c) :
Base(c) { Base(c) {
assertInvariants(); assertInvariants();
} }
/* ************************************************************************* */ /* ************************************************************************* */
#ifdef TRACK_ELIMINATE #ifdef TRACK_ELIMINATE
boost::shared_ptr<IndexConditional> IndexFactor::eliminateFirst() { boost::shared_ptr<IndexConditionalOrdered> IndexFactorOrdered::eliminateFirst() {
boost::shared_ptr<IndexConditional> result(Base::eliminateFirst< boost::shared_ptr<IndexConditionalOrdered> result(Base::eliminateFirst<
IndexConditional>()); IndexConditionalOrdered>());
assertInvariants(); assertInvariants();
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<BayesNet<IndexConditional> > IndexFactor::eliminate( boost::shared_ptr<BayesNetOrdered<IndexConditionalOrdered> > IndexFactorOrdered::eliminate(
size_t nrFrontals) { size_t nrFrontals) {
boost::shared_ptr<BayesNet<IndexConditional> > result(Base::eliminate< boost::shared_ptr<BayesNetOrdered<IndexConditionalOrdered> > result(Base::eliminate<
IndexConditional>(nrFrontals)); IndexConditionalOrdered>(nrFrontals));
assertInvariants(); assertInvariants();
return result; return result;
} }
#endif #endif
/* ************************************************************************* */ /* ************************************************************************* */
void IndexFactor::permuteWithInverse(const Permutation& inversePermutation) { void IndexFactorOrdered::permuteWithInverse(const Permutation& inversePermutation) {
BOOST_FOREACH(Index& key, keys()) BOOST_FOREACH(Index& key, keys())
key = inversePermutation[key]; key = inversePermutation[key];
assertInvariants(); assertInvariants();
} }
/* ************************************************************************* */ /* ************************************************************************* */
void IndexFactor::reduceWithInverse(const internal::Reduction& inverseReduction) { void IndexFactorOrdered::reduceWithInverse(const internal::Reduction& inverseReduction) {
BOOST_FOREACH(Index& key, keys()) BOOST_FOREACH(Index& key, keys())
key = inverseReduction[key]; key = inverseReduction[key];
assertInvariants(); assertInvariants();

44
gtsam/inference/IndexFactor.h → gtsam/inference/IndexFactorOrdered.h
View File

@ -17,13 +17,13 @@
#pragma once #pragma once
#include <gtsam/inference/Factor.h> #include <gtsam/inference/FactorOrdered.h>
#include <gtsam/inference/Permutation.h> #include <gtsam/inference/PermutationOrdered.h>
namespace gtsam { namespace gtsam {
// Forward declaration of IndexConditional // Forward declaration of IndexConditional
class IndexConditional; class IndexConditionalOrdered;
/** /**
* IndexFactor serves two purposes. It is the base class for all linear * IndexFactor serves two purposes. It is the base class for all linear
@ -34,7 +34,7 @@ namespace gtsam {
* It derives from Factor with a key type of Index, an unsigned integer. * It derives from Factor with a key type of Index, an unsigned integer.
* \nosubgrouping * \nosubgrouping
*/ */
class IndexFactor: public Factor<Index> { class IndexFactorOrdered: public FactorOrdered<Index> {
protected: protected:
@ -48,64 +48,64 @@ namespace gtsam {
public: public:
typedef IndexFactor This; typedef IndexFactorOrdered This;
typedef Factor<Index> Base; typedef FactorOrdered<Index> Base;
/** Elimination produces an IndexConditional */ /** Elimination produces an IndexConditional */
typedef IndexConditional ConditionalType; typedef IndexConditionalOrdered ConditionalType;
/** Overriding the shared_ptr typedef */ /** Overriding the shared_ptr typedef */
typedef boost::shared_ptr<IndexFactor> shared_ptr; typedef boost::shared_ptr<IndexFactorOrdered> shared_ptr;
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/** Copy constructor */ /** Copy constructor */
IndexFactor(const This& f) : IndexFactorOrdered(const This& f) :
Base(f) { Base(f) {
assertInvariants(); assertInvariants();
} }
/** Construct from derived type */ /** Construct from derived type */
GTSAM_EXPORT IndexFactor(const IndexConditional& c); GTSAM_EXPORT IndexFactorOrdered(const IndexConditionalOrdered& c);
/** Default constructor for I/O */ /** Default constructor for I/O */
IndexFactor() { IndexFactorOrdered() {
assertInvariants(); assertInvariants();
} }
/** Construct unary factor */ /** Construct unary factor */
IndexFactor(Index j) : IndexFactorOrdered(Index j) :
Base(j) { Base(j) {
assertInvariants(); assertInvariants();
} }
/** Construct binary factor */ /** Construct binary factor */
IndexFactor(Index j1, Index j2) : IndexFactorOrdered(Index j1, Index j2) :
Base(j1, j2) { Base(j1, j2) {
assertInvariants(); assertInvariants();
} }
/** Construct ternary factor */ /** Construct ternary factor */
IndexFactor(Index j1, Index j2, Index j3) : IndexFactorOrdered(Index j1, Index j2, Index j3) :
Base(j1, j2, j3) { Base(j1, j2, j3) {
assertInvariants(); assertInvariants();
} }
/** Construct 4-way factor */ /** Construct 4-way factor */
IndexFactor(Index j1, Index j2, Index j3, Index j4) : IndexFactorOrdered(Index j1, Index j2, Index j3, Index j4) :
Base(j1, j2, j3, j4) { Base(j1, j2, j3, j4) {
assertInvariants(); assertInvariants();
} }
/** Construct 5-way factor */ /** Construct 5-way factor */
IndexFactor(Index j1, Index j2, Index j3, Index j4, Index j5) : IndexFactorOrdered(Index j1, Index j2, Index j3, Index j4, Index j5) :
Base(j1, j2, j3, j4, j5) { Base(j1, j2, j3, j4, j5) {
assertInvariants(); assertInvariants();
} }
/** Construct 6-way factor */ /** Construct 6-way factor */
IndexFactor(Index j1, Index j2, Index j3, Index j4, Index j5, Index j6) : IndexFactorOrdered(Index j1, Index j2, Index j3, Index j4, Index j5, Index j6) :
Base(j1, j2, j3, j4, j5, j6) { Base(j1, j2, j3, j4, j5, j6) {
assertInvariants(); assertInvariants();
} }
@ -115,19 +115,19 @@ namespace gtsam {
/// @{ /// @{
/** Construct n-way factor */ /** Construct n-way factor */
IndexFactor(const std::set<Index>& js) : IndexFactorOrdered(const std::set<Index>& js) :
Base(js) { Base(js) {
assertInvariants(); assertInvariants();
} }
/** Construct n-way factor */ /** Construct n-way factor */
IndexFactor(const std::vector<Index>& js) : IndexFactorOrdered(const std::vector<Index>& js) :
Base(js) { Base(js) {
assertInvariants(); assertInvariants();
} }
/** Constructor from a collection of keys */ /** Constructor from a collection of keys */
template<class KeyIterator> IndexFactor(KeyIterator beginKey, template<class KeyIterator> IndexFactorOrdered(KeyIterator beginKey,
KeyIterator endKey) : KeyIterator endKey) :
Base(beginKey, endKey) { Base(beginKey, endKey) {
assertInvariants(); assertInvariants();
@ -143,7 +143,7 @@ namespace gtsam {
GTSAM_EXPORT boost::shared_ptr<ConditionalType> eliminateFirst(); GTSAM_EXPORT boost::shared_ptr<ConditionalType> eliminateFirst();
/** eliminate the first nrFrontals frontal variables.*/ /** eliminate the first nrFrontals frontal variables.*/
GTSAM_EXPORT boost::shared_ptr<BayesNet<ConditionalType> > eliminate(size_t nrFrontals = GTSAM_EXPORT boost::shared_ptr<BayesNetOrdered<ConditionalType> > eliminate(size_t nrFrontals =
1); 1);
#endif #endif
@ -161,7 +161,7 @@ namespace gtsam {
*/ */
GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction); GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction);
virtual ~IndexFactor() { virtual ~IndexFactorOrdered() {
} }
private: private:

42
gtsam/inference/JunctionTreeUnordered-inst.h → gtsam/inference/JunctionTree-inst.h
View File

@ -22,9 +22,9 @@
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h> #include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/inference/JunctionTreeUnordered.h> #include <gtsam/inference/JunctionTree.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h> #include <gtsam/symbolic/SymbolicConditional.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h> #include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/bind.hpp> #include <boost/bind.hpp>
@ -36,9 +36,9 @@ namespace gtsam {
template<class BAYESTREE, class GRAPH> template<class BAYESTREE, class GRAPH>
struct ConstructorTraversalData { struct ConstructorTraversalData {
ConstructorTraversalData* const parentData; ConstructorTraversalData* const parentData;
typename JunctionTreeUnordered<BAYESTREE,GRAPH>::sharedNode myJTNode; typename JunctionTree<BAYESTREE,GRAPH>::sharedNode myJTNode;
std::vector<SymbolicConditionalUnordered::shared_ptr> childSymbolicConditionals; std::vector<SymbolicConditional::shared_ptr> childSymbolicConditionals;
std::vector<SymbolicFactorUnordered::shared_ptr> childSymbolicFactors; std::vector<SymbolicFactor::shared_ptr> childSymbolicFactors;
ConstructorTraversalData(ConstructorTraversalData* _parentData) : parentData(_parentData) {} ConstructorTraversalData(ConstructorTraversalData* _parentData) : parentData(_parentData) {}
}; };
@ -52,7 +52,7 @@ namespace gtsam {
// On the pre-order pass, before children have been visited, we just set up a traversal data // On the pre-order pass, before children have been visited, we just set up a traversal data
// structure with its own JT node, and create a child pointer in its parent. // structure with its own JT node, and create a child pointer in its parent.
ConstructorTraversalData<BAYESTREE,GRAPH> myData = ConstructorTraversalData<BAYESTREE,GRAPH>(&parentData); ConstructorTraversalData<BAYESTREE,GRAPH> myData = ConstructorTraversalData<BAYESTREE,GRAPH>(&parentData);
myData.myJTNode = boost::make_shared<typename JunctionTreeUnordered<BAYESTREE,GRAPH>::Node>(); myData.myJTNode = boost::make_shared<typename JunctionTree<BAYESTREE,GRAPH>::Node>();
myData.myJTNode->keys.push_back(node->key); myData.myJTNode->keys.push_back(node->key);
myData.myJTNode->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end()); myData.myJTNode->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end());
parentData.myJTNode->children.push_back(myData.myJTNode); parentData.myJTNode->children.push_back(myData.myJTNode);
@ -74,17 +74,17 @@ namespace gtsam {
// current node did not introduce any parents beyond those already in the child. // current node did not introduce any parents beyond those already in the child.
// Do symbolic elimination for this node // Do symbolic elimination for this node
SymbolicFactorGraphUnordered symbolicFactors; SymbolicFactorGraph symbolicFactors;
symbolicFactors.reserve(ETreeNode->factors.size() + myData.childSymbolicFactors.size()); symbolicFactors.reserve(ETreeNode->factors.size() + myData.childSymbolicFactors.size());
// Add symbolic versions of the ETree node factors // Add symbolic versions of the ETree node factors
BOOST_FOREACH(const typename GRAPH::sharedFactor& factor, ETreeNode->factors) { BOOST_FOREACH(const typename GRAPH::sharedFactor& factor, ETreeNode->factors) {
symbolicFactors.push_back(boost::make_shared<SymbolicFactorUnordered>( symbolicFactors.push_back(boost::make_shared<SymbolicFactor>(
SymbolicFactorUnordered::FromKeys(*factor))); } SymbolicFactor::FromKeys(*factor))); }
// Add symbolic factors passed up from children // Add symbolic factors passed up from children
symbolicFactors.push_back(myData.childSymbolicFactors.begin(), myData.childSymbolicFactors.end()); symbolicFactors.push_back(myData.childSymbolicFactors.begin(), myData.childSymbolicFactors.end());
OrderingUnordered keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key); Ordering keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key);
std::pair<SymbolicConditionalUnordered::shared_ptr, SymbolicFactorUnordered::shared_ptr> symbolicElimResult = std::pair<SymbolicConditional::shared_ptr, SymbolicFactor::shared_ptr> symbolicElimResult =
EliminateSymbolicUnordered(symbolicFactors, keyAsOrdering); EliminateSymbolic(symbolicFactors, keyAsOrdering);
// Store symbolic elimination results in the parent // Store symbolic elimination results in the parent
myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first); myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first);
@ -102,7 +102,7 @@ namespace gtsam {
if(myNrParents + 1 == myData.childSymbolicConditionals[child]->nrParents()) { if(myNrParents + 1 == myData.childSymbolicConditionals[child]->nrParents()) {
// Get a reference to the child, adjusting the index to account for children previously // Get a reference to the child, adjusting the index to account for children previously
// merged and removed from the child list. // merged and removed from the child list.
const typename JunctionTreeUnordered<BAYESTREE,GRAPH>::Node& childToMerge = const typename JunctionTree<BAYESTREE,GRAPH>::Node& childToMerge =
*myData.myJTNode->children[child - nrMergedChildren]; *myData.myJTNode->children[child - nrMergedChildren];
// Merge keys, factors, and children. // Merge keys, factors, and children.
myData.myJTNode->keys.insert(myData.myJTNode->keys.begin(), childToMerge.keys.begin(), childToMerge.keys.end()); myData.myJTNode->keys.insert(myData.myJTNode->keys.begin(), childToMerge.keys.begin(), childToMerge.keys.end());
@ -182,7 +182,7 @@ namespace gtsam {
// Do dense elimination step // Do dense elimination step
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult = std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
eliminationFunction(gatheredFactors, OrderingUnordered(node->keys)); eliminationFunction(gatheredFactors, Ordering(node->keys));
// Store conditional in BayesTree clique // Store conditional in BayesTree clique
myData.bayesTreeNode->conditional_ = eliminationResult.first; myData.bayesTreeNode->conditional_ = eliminationResult.first;
@ -195,7 +195,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESTREE, class GRAPH> template<class BAYESTREE, class GRAPH>
void JunctionTreeUnordered<BAYESTREE,GRAPH>::Node::print( void JunctionTree<BAYESTREE,GRAPH>::Node::print(
const std::string& s, const KeyFormatter& keyFormatter) const const std::string& s, const KeyFormatter& keyFormatter) const
{ {
std::cout << s; std::cout << s;
@ -206,7 +206,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESTREE, class GRAPH> template<class BAYESTREE, class GRAPH>
void JunctionTreeUnordered<BAYESTREE,GRAPH>::print( void JunctionTree<BAYESTREE,GRAPH>::print(
const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const
{ {
treeTraversal::PrintForest(*this, s, keyFormatter); treeTraversal::PrintForest(*this, s, keyFormatter);
@ -215,8 +215,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESTREE, class GRAPH> template<class BAYESTREE, class GRAPH>
template<class ETREE> template<class ETREE>
JunctionTreeUnordered<BAYESTREE,GRAPH> JunctionTree<BAYESTREE,GRAPH>
JunctionTreeUnordered<BAYESTREE,GRAPH>::FromEliminationTree(const ETREE& eliminationTree) JunctionTree<BAYESTREE,GRAPH>::FromEliminationTree(const ETREE& eliminationTree)
{ {
gttic(JunctionTree_FromEliminationTree); gttic(JunctionTree_FromEliminationTree);
// Here we rely on the BayesNet having been produced by this elimination tree, such that the // Here we rely on the BayesNet having been produced by this elimination tree, such that the
@ -245,7 +245,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESTREE, class GRAPH> template<class BAYESTREE, class GRAPH>
JunctionTreeUnordered<BAYESTREE,GRAPH>& JunctionTreeUnordered<BAYESTREE,GRAPH>::operator=(const This& other) JunctionTree<BAYESTREE,GRAPH>& JunctionTree<BAYESTREE,GRAPH>::operator=(const This& other)
{ {
// Start by duplicating the tree. // Start by duplicating the tree.
roots_ = treeTraversal::CloneForest(other); roots_ = treeTraversal::CloneForest(other);
@ -260,7 +260,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESTREE, class GRAPH> template<class BAYESTREE, class GRAPH>
std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> > std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> >
JunctionTreeUnordered<BAYESTREE,GRAPH>::eliminate(const Eliminate& function) const JunctionTree<BAYESTREE,GRAPH>::eliminate(const Eliminate& function) const
{ {
gttic(JunctionTree_eliminate); gttic(JunctionTree_eliminate);
// Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node

141
gtsam/inference/JunctionTree.h
View File

@ -14,18 +14,14 @@
* @date Feb 4, 2010 * @date Feb 4, 2010
* @author Kai Ni * @author Kai Ni
* @author Frank Dellaert * @author Frank Dellaert
* @brief: The junction tree * @author Richard Roberts
* @brief The junction tree
*/ */
#pragma once #pragma once
#include <set> #include <gtsam/base/Testable.h>
#include <vector> #include <gtsam/inference/Key.h>
#include <gtsam/base/FastList.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/ClusterTree.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/VariableIndex.h>
namespace gtsam { namespace gtsam {
@ -46,90 +42,103 @@ namespace gtsam {
* The tree structure and elimination method are exactly analagous to the EliminationTree, * The tree structure and elimination method are exactly analagous to the EliminationTree,
* except that in the JunctionTree, at each node multiple variables are eliminated at a time. * except that in the JunctionTree, at each node multiple variables are eliminated at a time.
* *
*
* \addtogroup Multifrontal * \addtogroup Multifrontal
* \nosubgrouping * \nosubgrouping
*/ */
template<class FG, class BTCLIQUE=typename BayesTree<typename FG::FactorType::ConditionalType>::Clique> template<class BAYESTREE, class GRAPH>
class JunctionTree: public ClusterTree<FG> { class JunctionTree {
public: public:
/// In a junction tree each cluster is associated with a clique typedef GRAPH FactorGraphType; ///< The factor graph type
typedef typename ClusterTree<FG>::Cluster Clique; typedef typename GRAPH::FactorType FactorType; ///< The type of factors
typedef typename Clique::shared_ptr sharedClique; ///< Shared pointer to a clique typedef JunctionTree<BAYESTREE, GRAPH> This; ///< This class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
typedef boost::shared_ptr<FactorType> sharedFactor; ///< Shared pointer to a factor
typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination
typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals
typedef boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine
/// The BayesTree type produced by elimination struct Node {
typedef BTCLIQUE BTClique; typedef std::vector<Key> Keys;
typedef std::vector<sharedFactor> Factors;
typedef std::vector<boost::shared_ptr<Node> > Children;
/// Shared pointer to this class Keys keys; ///< Frontal keys of this node
typedef boost::shared_ptr<JunctionTree<FG> > shared_ptr; Factors factors; ///< Factors associated with this node
Children children; ///< sub-trees
int problemSize_;
/// We will frequently refer to a symbolic Bayes tree, used to find the clique structure int problemSize() const { return problemSize_; }
typedef gtsam::BayesTree<IndexConditional> SymbolicBayesTree;
/** print this node */
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
};
typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
private: private:
/// @name Advanced Interface /** concept check */
/// @{ GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
/// distribute the factors along the cluster tree std::vector<sharedNode> roots_;
sharedClique distributeFactors(const FG& fg, std::vector<sharedFactor> remainingFactors_;
const SymbolicBayesTree::sharedClique& clique);
/// distribute the factors along the cluster tree protected:
sharedClique distributeFactors(const FG& fg, const std::vector<FastList<size_t> >& targets,
const SymbolicBayesTree::sharedClique& clique);
/// recursive elimination function
std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor>
eliminateOneClique(typename FG::Eliminate function,
const boost::shared_ptr<const Clique>& clique) const;
/// internal constructor
void construct(const FG& fg, const VariableIndex& variableIndex);
/// @}
public:
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
/** Default constructor */ /** Build the junction tree from an elimination tree. */
JunctionTree() {} template<class ETREE>
static This FromEliminationTree(const ETREE& eliminationTree);
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
* copied, factors are not cloned. */
JunctionTree(const This& other) { *this = other; }
/** Named constructor to build the junction tree of a factor graph. Note /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors
* that this has to compute the column structure as a VariableIndex, so if you * are copied, factors are not cloned. */
* already have this precomputed, use the JunctionTree(const FG&, const VariableIndex&) This& operator=(const This& other);
* constructor instead.
* @param factorGraph The factor graph for which to build the elimination tree
*/
JunctionTree(const FG& factorGraph);
/** Construct from a factor graph and pre-computed variable index.
* @param fg The factor graph for which to build the junction tree
* @param structure The set of factors involving each variable. If this is not
* precomputed, you can call the JunctionTree(const FG&)
* constructor instead.
*/
JunctionTree(const FG& fg, const VariableIndex& variableIndex);
/// @} /// @}
public:
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/** Eliminate the factors in the subgraphs to produce a BayesTree. /** Eliminate the factors to a Bayes net and remaining factor graph
* @param function The function used to eliminate, see the namespace functions * @param function The function to use to eliminate, see the namespace functions
* in GaussianFactorGraph.h * in GaussianFactorGraph.h
* @return The BayesTree resulting from elimination * @return The Bayes net and factor graph resulting from elimination
*/ */
typename BTClique::shared_ptr eliminate(typename FG::Eliminate function) const; std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
eliminate(const Eliminate& function) const;
/** Print the junction tree */
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @} /// @}
}; // JunctionTree /// @name Advanced Interface
/// @{
/** Return the set of roots (one for a tree, multiple for a forest) */
const std::vector<sharedNode>& roots() const { return roots_; }
} // namespace gtsam /** Return the remaining factors that are not pulled into elimination */
const std::vector<sharedFactor>& remainingFactors() const { return remainingFactors_; }
#include <gtsam/inference/JunctionTree-inl.h> /// @}
private:
// Private default constructor (used in static construction methods)
JunctionTree() {}
};
}

32
gtsam/inference/JunctionTree-inl.h → gtsam/inference/JunctionTreeOrdered-inl.h
View File

@ -19,9 +19,9 @@
#pragma once #pragma once
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/VariableSlots.h> #include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/EliminationTree.h> #include <gtsam/inference/EliminationTreeOrdered.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
@ -30,15 +30,15 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template <class FG, class BTCLIQUE> template <class FG, class BTCLIQUE>
void JunctionTree<FG,BTCLIQUE>::construct(const FG& fg, const VariableIndex& variableIndex) { void JunctionTreeOrdered<FG,BTCLIQUE>::construct(const FG& fg, const VariableIndexOrdered& variableIndex) {
gttic(JT_construct); gttic(JT_construct);
gttic(JT_symbolic_ET); gttic(JT_symbolic_ET);
const typename EliminationTree<IndexFactor>::shared_ptr symETree = const typename EliminationTreeOrdered<IndexFactorOrdered>::shared_ptr symETree =
EliminationTree<IndexFactor>::Create(fg, variableIndex); EliminationTreeOrdered<IndexFactorOrdered>::Create(fg, variableIndex);
assert(symETree.get()); assert(symETree.get());
gttoc(JT_symbolic_ET); gttoc(JT_symbolic_ET);
gttic(JT_symbolic_eliminate); gttic(JT_symbolic_eliminate);
SymbolicBayesNet::shared_ptr sbn = symETree->eliminate(&EliminateSymbolic); SymbolicBayesNetOrdered::shared_ptr sbn = symETree->eliminate(&EliminateSymbolic);
assert(sbn.get()); assert(sbn.get());
gttoc(JT_symbolic_eliminate); gttoc(JT_symbolic_eliminate);
gttic(symbolic_BayesTree); gttic(symbolic_BayesTree);
@ -53,22 +53,22 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template <class FG, class BTCLIQUE> template <class FG, class BTCLIQUE>
JunctionTree<FG,BTCLIQUE>::JunctionTree(const FG& fg) { JunctionTreeOrdered<FG,BTCLIQUE>::JunctionTreeOrdered(const FG& fg) {
gttic(VariableIndex); gttic(VariableIndexOrdered);
VariableIndex varIndex(fg); VariableIndexOrdered varIndex(fg);
gttoc(VariableIndex); gttoc(VariableIndexOrdered);
construct(fg, varIndex); construct(fg, varIndex);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <class FG, class BTCLIQUE> template <class FG, class BTCLIQUE>
JunctionTree<FG,BTCLIQUE>::JunctionTree(const FG& fg, const VariableIndex& variableIndex) { JunctionTreeOrdered<FG,BTCLIQUE>::JunctionTreeOrdered(const FG& fg, const VariableIndexOrdered& variableIndex) {
construct(fg, variableIndex); construct(fg, variableIndex);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG, class BTCLIQUE> template<class FG, class BTCLIQUE>
typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors( typename JunctionTreeOrdered<FG,BTCLIQUE>::sharedClique JunctionTreeOrdered<FG,BTCLIQUE>::distributeFactors(
const FG& fg, const SymbolicBayesTree::sharedClique& bayesClique) { const FG& fg, const SymbolicBayesTree::sharedClique& bayesClique) {
// Build "target" index. This is an index for each variable of the factors // Build "target" index. This is an index for each variable of the factors
@ -104,7 +104,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG, class BTCLIQUE> template<class FG, class BTCLIQUE>
typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors(const FG& fg, typename JunctionTreeOrdered<FG,BTCLIQUE>::sharedClique JunctionTreeOrdered<FG,BTCLIQUE>::distributeFactors(const FG& fg,
const std::vector<FastList<size_t> >& targets, const std::vector<FastList<size_t> >& targets,
const SymbolicBayesTree::sharedClique& bayesClique) { const SymbolicBayesTree::sharedClique& bayesClique) {
@ -142,8 +142,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG, class BTCLIQUE> template<class FG, class BTCLIQUE>
std::pair<typename JunctionTree<FG,BTCLIQUE>::BTClique::shared_ptr, std::pair<typename JunctionTreeOrdered<FG,BTCLIQUE>::BTClique::shared_ptr,
typename FG::sharedFactor> JunctionTree<FG,BTCLIQUE>::eliminateOneClique( typename FG::sharedFactor> JunctionTreeOrdered<FG,BTCLIQUE>::eliminateOneClique(
typename FG::Eliminate function, typename FG::Eliminate function,
const boost::shared_ptr<const Clique>& current) const { const boost::shared_ptr<const Clique>& current) const {
@ -189,7 +189,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG, class BTCLIQUE> template<class FG, class BTCLIQUE>
typename BTCLIQUE::shared_ptr JunctionTree<FG,BTCLIQUE>::eliminate( typename BTCLIQUE::shared_ptr JunctionTreeOrdered<FG,BTCLIQUE>::eliminate(
typename FG::Eliminate function) const typename FG::Eliminate function) const
{ {
if (this->root()) { if (this->root()) {

135
gtsam/inference/JunctionTreeOrdered.h Normal file
View File

@ -0,0 +1,135 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file JunctionTree.h
* @date Feb 4, 2010
* @author Kai Ni
* @author Frank Dellaert
* @brief: The junction tree
*/
#pragma once
#include <set>
#include <vector>
#include <gtsam/base/FastList.h>
#include <gtsam/inference/BayesTreeOrdered.h>
#include <gtsam/inference/ClusterTreeOrdered.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#include <gtsam/inference/VariableIndexOrdered.h>
namespace gtsam {
/**
* A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with
* the additional property that it represents the clique tree associated with a Bayes net.
*
* In GTSAM a junction tree is an intermediate data structure in multifrontal
* variable elimination. Each node is a cluster of factors, along with a
* clique of variables that are eliminated all at once. In detail, every node k represents
* a clique (maximal fully connected subset) of an associated chordal graph, such as a
* chordal Bayes net resulting from elimination.
*
* The difference with the BayesTree is that a JunctionTree stores factors, whereas a
* BayesTree stores conditionals, that are the product of eliminating the factors in the
* corresponding JunctionTree cliques.
*
* The tree structure and elimination method are exactly analagous to the EliminationTree,
* except that in the JunctionTree, at each node multiple variables are eliminated at a time.
*
*
* \addtogroup Multifrontal
* \nosubgrouping
*/
template<class FG, class BTCLIQUE=typename BayesTreeOrdered<typename FG::FactorType::ConditionalType>::Clique>
class JunctionTreeOrdered: public ClusterTreeOrdered<FG> {
public:
/// In a junction tree each cluster is associated with a clique
typedef typename ClusterTreeOrdered<FG>::Cluster Clique;
typedef typename Clique::shared_ptr sharedClique; ///< Shared pointer to a clique
/// The BayesTree type produced by elimination
typedef BTCLIQUE BTClique;
/// Shared pointer to this class
typedef boost::shared_ptr<JunctionTreeOrdered<FG> > shared_ptr;
/// We will frequently refer to a symbolic Bayes tree, used to find the clique structure
typedef gtsam::BayesTreeOrdered<IndexConditionalOrdered> SymbolicBayesTree;
private:
/// @name Advanced Interface
/// @{
/// distribute the factors along the cluster tree
sharedClique distributeFactors(const FG& fg,
const SymbolicBayesTree::sharedClique& clique);
/// distribute the factors along the cluster tree
sharedClique distributeFactors(const FG& fg, const std::vector<FastList<size_t> >& targets,
const SymbolicBayesTree::sharedClique& clique);
/// recursive elimination function
std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor>
eliminateOneClique(typename FG::Eliminate function,
const boost::shared_ptr<const Clique>& clique) const;
/// internal constructor
void construct(const FG& fg, const VariableIndexOrdered& variableIndex);
/// @}
public:
/// @name Standard Constructors
/// @{
/** Default constructor */
JunctionTreeOrdered() {}
/** Named constructor to build the junction tree of a factor graph. Note
* that this has to compute the column structure as a VariableIndex, so if you
* already have this precomputed, use the JunctionTree(const FG&, const VariableIndex&)
* constructor instead.
* @param factorGraph The factor graph for which to build the elimination tree
*/
JunctionTreeOrdered(const FG& factorGraph);
/** Construct from a factor graph and pre-computed variable index.
* @param fg The factor graph for which to build the junction tree
* @param structure The set of factors involving each variable. If this is not
* precomputed, you can call the JunctionTree(const FG&)
* constructor instead.
*/
JunctionTreeOrdered(const FG& fg, const VariableIndexOrdered& variableIndex);
/// @}
/// @name Standard Interface
/// @{
/** Eliminate the factors in the subgraphs to produce a BayesTree.
* @param function The function used to eliminate, see the namespace functions
* in GaussianFactorGraph.h
* @return The BayesTree resulting from elimination
*/
typename BTClique::shared_ptr eliminate(typename FG::Eliminate function) const;
/// @}
}; // JunctionTree
} // namespace gtsam
#include <gtsam/inference/JunctionTreeOrdered-inl.h>

144
gtsam/inference/JunctionTreeUnordered.h
View File

@ -1,144 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file JunctionTree.h
* @date Feb 4, 2010
* @author Kai Ni
* @author Frank Dellaert
* @author Richard Roberts
* @brief The junction tree
*/
#pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
namespace gtsam {
/**
* A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with
* the additional property that it represents the clique tree associated with a Bayes net.
*
* In GTSAM a junction tree is an intermediate data structure in multifrontal
* variable elimination. Each node is a cluster of factors, along with a
* clique of variables that are eliminated all at once. In detail, every node k represents
* a clique (maximal fully connected subset) of an associated chordal graph, such as a
* chordal Bayes net resulting from elimination.
*
* The difference with the BayesTree is that a JunctionTree stores factors, whereas a
* BayesTree stores conditionals, that are the product of eliminating the factors in the
* corresponding JunctionTree cliques.
*
* The tree structure and elimination method are exactly analagous to the EliminationTree,
* except that in the JunctionTree, at each node multiple variables are eliminated at a time.
*
* \addtogroup Multifrontal
* \nosubgrouping
*/
template<class BAYESTREE, class GRAPH>
class JunctionTreeUnordered {
public:
typedef GRAPH FactorGraphType; ///< The factor graph type
typedef typename GRAPH::FactorType FactorType; ///< The type of factors
typedef JunctionTreeUnordered<BAYESTREE, GRAPH> This; ///< This class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
typedef boost::shared_ptr<FactorType> sharedFactor; ///< Shared pointer to a factor
typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination
typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals
typedef boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine
struct Node {
typedef std::vector<Key> Keys;
typedef std::vector<sharedFactor> Factors;
typedef std::vector<boost::shared_ptr<Node> > Children;
Keys keys; ///< Frontal keys of this node
Factors factors; ///< Factors associated with this node
Children children; ///< sub-trees
int problemSize_;
int problemSize() const { return problemSize_; }
/** print this node */
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
};
typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
private:
/** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
std::vector<sharedNode> roots_;
std::vector<sharedFactor> remainingFactors_;
protected:
/// @name Standard Constructors
/// @{
/** Build the junction tree from an elimination tree. */
template<class ETREE>
static This FromEliminationTree(const ETREE& eliminationTree);
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
* copied, factors are not cloned. */
JunctionTreeUnordered(const This& other) { *this = other; }
/** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors
* are copied, factors are not cloned. */
This& operator=(const This& other);
/// @}
public:
/// @name Standard Interface
/// @{
/** Eliminate the factors to a Bayes net and remaining factor graph
* @param function The function to use to eliminate, see the namespace functions
* in GaussianFactorGraph.h
* @return The Bayes net and factor graph resulting from elimination
*/
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
eliminate(const Eliminate& function) const;
/** Print the junction tree */
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @}
/// @name Advanced Interface
/// @{
/** Return the set of roots (one for a tree, multiple for a forest) */
const std::vector<sharedNode>& roots() const { return roots_; }
/** Return the remaining factors that are not pulled into elimination */
const std::vector<sharedFactor>& remainingFactors() const { return remainingFactors_; }
/// @}
private:
// Private default constructor (used in static construction methods)
JunctionTreeUnordered() {}
};
}

30
gtsam/inference/OrderingUnordered.cpp → gtsam/inference/Ordering.cpp
View File

@ -20,7 +20,7 @@
#include <vector> #include <vector>
#include <limits> #include <limits>
#include <gtsam/inference/OrderingUnordered.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h> #include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>
using namespace std; using namespace std;
@ -28,7 +28,7 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
FastMap<Key, size_t> OrderingUnordered::invert() const FastMap<Key, size_t> Ordering::invert() const
{ {
FastMap<Key, size_t> inverted; FastMap<Key, size_t> inverted;
for(size_t pos = 0; pos < this->size(); ++pos) for(size_t pos = 0; pos < this->size(); ++pos)
@ -37,17 +37,17 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
OrderingUnordered OrderingUnordered::COLAMD(const VariableIndexUnordered& variableIndex) Ordering Ordering::COLAMD(const VariableIndex& variableIndex)
{ {
// Call constrained version with all groups set to zero // Call constrained version with all groups set to zero
return OrderingUnordered::COLAMDConstrained(variableIndex, vector<int>(variableIndex.size(), 0)); return Ordering::COLAMDConstrained(variableIndex, vector<int>(variableIndex.size(), 0));
} }
/* ************************************************************************* */ /* ************************************************************************* */
OrderingUnordered OrderingUnordered::COLAMDConstrained( Ordering Ordering::COLAMDConstrained(
const VariableIndexUnordered& variableIndex, std::vector<int>& cmember) const VariableIndex& variableIndex, std::vector<int>& cmember)
{ {
gttic(OrderingUnordered_COLAMDConstrained); gttic(Ordering_COLAMDConstrained);
gttic(Prepare); gttic(Prepare);
size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
@ -61,9 +61,9 @@ namespace gtsam {
int count = 0; int count = 0;
vector<Key> keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order vector<Key> keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order
size_t index = 0; size_t index = 0;
BOOST_FOREACH(const VariableIndexUnordered::value_type key_factors, variableIndex) { BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) {
// Arrange factor indices into COLAMD format // Arrange factor indices into COLAMD format
const VariableIndexUnordered::Factors& column = key_factors.second; const VariableIndex::Factors& column = key_factors.second;
size_t lastFactorId = numeric_limits<size_t>::max(); size_t lastFactorId = numeric_limits<size_t>::max();
BOOST_FOREACH(size_t factorIndex, column) { BOOST_FOREACH(size_t factorIndex, column) {
if(lastFactorId != numeric_limits<size_t>::max()) if(lastFactorId != numeric_limits<size_t>::max())
@ -101,7 +101,7 @@ namespace gtsam {
gttic(Fill_Ordering); gttic(Fill_Ordering);
// Convert elimination ordering in p to an ordering // Convert elimination ordering in p to an ordering
OrderingUnordered result; Ordering result;
result.resize(nVars); result.resize(nVars);
for(size_t j = 0; j < nVars; ++j) for(size_t j = 0; j < nVars; ++j)
result[j] = keys[p[j]]; result[j] = keys[p[j]];
@ -111,10 +111,10 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
OrderingUnordered OrderingUnordered::COLAMDConstrainedLast( Ordering Ordering::COLAMDConstrainedLast(
const VariableIndexUnordered& variableIndex, const std::vector<Key>& constrainLast, bool forceOrder) const VariableIndex& variableIndex, const std::vector<Key>& constrainLast, bool forceOrder)
{ {
gttic(OrderingUnordered_COLAMDConstrainedLast); gttic(Ordering_COLAMDConstrainedLast);
size_t n = variableIndex.size(); size_t n = variableIndex.size();
std::vector<int> cmember(n, 0); std::vector<int> cmember(n, 0);
@ -122,7 +122,7 @@ namespace gtsam {
// Build a mapping to look up sorted Key indices by Key // Build a mapping to look up sorted Key indices by Key
FastMap<Key, size_t> keyIndices; FastMap<Key, size_t> keyIndices;
size_t j = 0; size_t j = 0;
BOOST_FOREACH(const VariableIndexUnordered::value_type key_factors, variableIndex) BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex)
keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++));
// If at least some variables are not constrained to be last, constrain the // If at least some variables are not constrained to be last, constrain the
@ -134,7 +134,7 @@ namespace gtsam {
++ group; ++ group;
} }
return OrderingUnordered::COLAMDConstrained(variableIndex, cmember); return Ordering::COLAMDConstrained(variableIndex, cmember);
} }
} }

28
gtsam/inference/OrderingUnordered.h → gtsam/inference/Ordering.h
View File

@ -20,45 +20,45 @@
#include <vector> #include <vector>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/inference/VariableIndexUnordered.h> #include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/FactorGraphUnordered.h> #include <gtsam/inference/FactorGraph.h>
namespace gtsam { namespace gtsam {
class OrderingUnordered : public std::vector<Key> { class Ordering : public std::vector<Key> {
protected: protected:
typedef std::vector<Key> Base; typedef std::vector<Key> Base;
public: public:
/// Create an empty ordering /// Create an empty ordering
GTSAM_EXPORT OrderingUnordered() {} GTSAM_EXPORT Ordering() {}
/// Create from a container /// Create from a container
template<typename KEYS> template<typename KEYS>
explicit OrderingUnordered(const KEYS& keys) : Base(keys.begin(), keys.end()) {} explicit Ordering(const KEYS& keys) : Base(keys.begin(), keys.end()) {}
/// Create an ordering using iterators over keys /// Create an ordering using iterators over keys
template<typename ITERATOR> template<typename ITERATOR>
OrderingUnordered(ITERATOR firstKey, ITERATOR lastKey) : Base(firstKey, lastKey) {} Ordering(ITERATOR firstKey, ITERATOR lastKey) : Base(firstKey, lastKey) {}
/// Invert (not reverse) the ordering - returns a map from key to order position /// Invert (not reverse) the ordering - returns a map from key to order position
FastMap<Key, size_t> invert() const; FastMap<Key, size_t> invert() const;
/// Compute an ordering using COLAMD directly from a factor graph - this internally builds a /// Compute an ordering using COLAMD directly from a factor graph - this internally builds a
/// VariableIndex so if you already have a VariableIndex, it is faster to use COLAMD(const /// VariableIndex so if you already have a VariableIndex, it is faster to use COLAMD(const
/// VariableIndexUnordered&) /// VariableIndex&)
template<class FACTOR> template<class FACTOR>
static OrderingUnordered COLAMD(const FactorGraphUnordered<FACTOR>& graph) { static Ordering COLAMD(const FactorGraph<FACTOR>& graph) {
return COLAMD(VariableIndexUnordered(graph)); return COLAMD(VariableIndex(graph));
} }
static GTSAM_EXPORT OrderingUnordered COLAMD(const VariableIndexUnordered& variableIndex); static GTSAM_EXPORT Ordering COLAMD(const VariableIndex& variableIndex);
static GTSAM_EXPORT OrderingUnordered COLAMDConstrainedLast( static GTSAM_EXPORT Ordering COLAMDConstrainedLast(
const VariableIndexUnordered& variableIndex, const std::vector<Key>& constrainLast, bool forceOrder = false); const VariableIndex& variableIndex, const std::vector<Key>& constrainLast, bool forceOrder = false);
private: private:
static GTSAM_EXPORT OrderingUnordered COLAMDConstrained( static GTSAM_EXPORT Ordering COLAMDConstrained(
const VariableIndexUnordered& variableIndex, const VariableIndex& variableIndex,
std::vector<int>& cmember); std::vector<int>& cmember);
}; };
} }

2
gtsam/inference/Permutation.cpp → gtsam/inference/PermutationOrdered.cpp
View File

@ -15,7 +15,7 @@
* @date Oct 9, 2010 * @date Oct 9, 2010
*/ */
#include <gtsam/inference/Permutation.h> #include <gtsam/inference/PermutationOrdered.h>
#include <iostream> #include <iostream>
#include <sstream> #include <sstream>

0
gtsam/inference/Permutation.h → gtsam/inference/PermutationOrdered.h
View File

132
gtsam/inference/SymbolicFactorGraph.cpp
View File

@ -1,132 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SymbolicFactorGraph.cpp
* @date Oct 29, 2009
* @author Frank Dellaert
*/
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/inference/IndexConditional.h>
#include <boost/make_shared.hpp>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
SymbolicFactorGraph::SymbolicFactorGraph(const SymbolicBayesNet& bayesNet) :
FactorGraph<IndexFactor>(bayesNet) {}
/* ************************************************************************* */
SymbolicFactorGraph::SymbolicFactorGraph(const SymbolicBayesTree& bayesTree) :
FactorGraph<IndexFactor>(bayesTree) {}
/* ************************************************************************* */
void SymbolicFactorGraph::push_factor(Index key) {
push_back(boost::make_shared<IndexFactor>(key));
}
/** Push back binary factor */
void SymbolicFactorGraph::push_factor(Index key1, Index key2) {
push_back(boost::make_shared<IndexFactor>(key1,key2));
}
/** Push back ternary factor */
void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3) {
push_back(boost::make_shared<IndexFactor>(key1,key2,key3));
}
/** Push back 4-way factor */
void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3, Index key4) {
push_back(boost::make_shared<IndexFactor>(key1,key2,key3,key4));
}
/* ************************************************************************* */
FastSet<Index>
SymbolicFactorGraph::keys() const {
FastSet<Index> keys;
BOOST_FOREACH(const sharedFactor& factor, *this) {
if(factor) keys.insert(factor->begin(), factor->end()); }
return keys;
}
/* ************************************************************************* */
std::pair<SymbolicFactorGraph::sharedConditional, SymbolicFactorGraph>
SymbolicFactorGraph::eliminateFrontals(size_t nFrontals) const
{
return FactorGraph<IndexFactor>::eliminateFrontals(nFrontals, EliminateSymbolic);
}
/* ************************************************************************* */
std::pair<SymbolicFactorGraph::sharedConditional, SymbolicFactorGraph>
SymbolicFactorGraph::eliminate(const std::vector<Index>& variables) const
{
return FactorGraph<IndexFactor>::eliminate(variables, EliminateSymbolic);
}
/* ************************************************************************* */
std::pair<SymbolicFactorGraph::sharedConditional, SymbolicFactorGraph>
SymbolicFactorGraph::eliminateOne(Index variable) const
{
return FactorGraph<IndexFactor>::eliminateOne(variable, EliminateSymbolic);
}
/* ************************************************************************* */
void SymbolicFactorGraph::permuteWithInverse(
const Permutation& inversePermutation) {
BOOST_FOREACH(const sharedFactor& factor, factors_) {
if(factor)
factor->permuteWithInverse(inversePermutation);
}
}
/* ************************************************************************* */
void SymbolicFactorGraph::reduceWithInverse(
const internal::Reduction& inverseReduction) {
BOOST_FOREACH(const sharedFactor& factor, factors_) {
if(factor)
factor->reduceWithInverse(inverseReduction);
}
}
/* ************************************************************************* */
IndexFactor::shared_ptr CombineSymbolic(
const FactorGraph<IndexFactor>& factors, const FastMap<Index,
vector<Index> >& variableSlots) {
IndexFactor::shared_ptr combined(Combine<IndexFactor, Index> (factors, variableSlots));
// combined->assertInvariants();
return combined;
}
/* ************************************************************************* */
pair<IndexConditional::shared_ptr, IndexFactor::shared_ptr> //
EliminateSymbolic(const FactorGraph<IndexFactor>& factors, size_t nrFrontals) {
FastSet<Index> keys;
BOOST_FOREACH(const IndexFactor::shared_ptr& factor, factors)
BOOST_FOREACH(Index var, *factor)
keys.insert(var);
if (keys.size() < nrFrontals) throw invalid_argument(
"EliminateSymbolic requested to eliminate more variables than exist in graph.");
vector<Index> newKeys(keys.begin(), keys.end());
return make_pair(boost::make_shared<IndexConditional>(newKeys, nrFrontals),
boost::make_shared<IndexFactor>(newKeys.begin() + nrFrontals, newKeys.end()));
}
/* ************************************************************************* */
}

132
gtsam/inference/SymbolicFactorGraphOrdered.cpp Normal file
View File

@ -0,0 +1,132 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SymbolicFactorGraph.cpp
* @date Oct 29, 2009
* @author Frank Dellaert
*/
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/BayesNetOrdered.h>
#include <gtsam/inference/EliminationTreeOrdered.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#include <boost/make_shared.hpp>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
SymbolicFactorGraphOrdered::SymbolicFactorGraphOrdered(const SymbolicBayesNetOrdered& bayesNet) :
FactorGraphOrdered<IndexFactorOrdered>(bayesNet) {}
/* ************************************************************************* */
SymbolicFactorGraphOrdered::SymbolicFactorGraphOrdered(const SymbolicBayesTreeOrdered& bayesTree) :
FactorGraphOrdered<IndexFactorOrdered>(bayesTree) {}
/* ************************************************************************* */
void SymbolicFactorGraphOrdered::push_factor(Index key) {
push_back(boost::make_shared<IndexFactorOrdered>(key));
}
/** Push back binary factor */
void SymbolicFactorGraphOrdered::push_factor(Index key1, Index key2) {
push_back(boost::make_shared<IndexFactorOrdered>(key1,key2));
}
/** Push back ternary factor */
void SymbolicFactorGraphOrdered::push_factor(Index key1, Index key2, Index key3) {
push_back(boost::make_shared<IndexFactorOrdered>(key1,key2,key3));
}
/** Push back 4-way factor */
void SymbolicFactorGraphOrdered::push_factor(Index key1, Index key2, Index key3, Index key4) {
push_back(boost::make_shared<IndexFactorOrdered>(key1,key2,key3,key4));
}
/* ************************************************************************* */
FastSet<Index>
SymbolicFactorGraphOrdered::keys() const {
FastSet<Index> keys;
BOOST_FOREACH(const sharedFactor& factor, *this) {
if(factor) keys.insert(factor->begin(), factor->end()); }
return keys;
}
/* ************************************************************************* */
std::pair<SymbolicFactorGraphOrdered::sharedConditional, SymbolicFactorGraphOrdered>
SymbolicFactorGraphOrdered::eliminateFrontals(size_t nFrontals) const
{
return FactorGraphOrdered<IndexFactorOrdered>::eliminateFrontals(nFrontals, EliminateSymbolic);
}
/* ************************************************************************* */
std::pair<SymbolicFactorGraphOrdered::sharedConditional, SymbolicFactorGraphOrdered>
SymbolicFactorGraphOrdered::eliminate(const std::vector<Index>& variables) const
{
return FactorGraphOrdered<IndexFactorOrdered>::eliminate(variables, EliminateSymbolic);
}
/* ************************************************************************* */
std::pair<SymbolicFactorGraphOrdered::sharedConditional, SymbolicFactorGraphOrdered>
SymbolicFactorGraphOrdered::eliminateOne(Index variable) const
{
return FactorGraphOrdered<IndexFactorOrdered>::eliminateOne(variable, EliminateSymbolic);
}
/* ************************************************************************* */
void SymbolicFactorGraphOrdered::permuteWithInverse(
const Permutation& inversePermutation) {
BOOST_FOREACH(const sharedFactor& factor, factors_) {
if(factor)
factor->permuteWithInverse(inversePermutation);
}
}
/* ************************************************************************* */
void SymbolicFactorGraphOrdered::reduceWithInverse(
const internal::Reduction& inverseReduction) {
BOOST_FOREACH(const sharedFactor& factor, factors_) {
if(factor)
factor->reduceWithInverse(inverseReduction);
}
}
/* ************************************************************************* */
IndexFactorOrdered::shared_ptr CombineSymbolic(
const FactorGraphOrdered<IndexFactorOrdered>& factors, const FastMap<Index,
vector<Index> >& variableSlots) {
IndexFactorOrdered::shared_ptr combined(Combine<IndexFactorOrdered, Index> (factors, variableSlots));
// combined->assertInvariants();
return combined;
}
/* ************************************************************************* */
pair<IndexConditionalOrdered::shared_ptr, IndexFactorOrdered::shared_ptr> //
EliminateSymbolic(const FactorGraphOrdered<IndexFactorOrdered>& factors, size_t nrFrontals) {
FastSet<Index> keys;
BOOST_FOREACH(const IndexFactorOrdered::shared_ptr& factor, factors)
BOOST_FOREACH(Index var, *factor)
keys.insert(var);
if (keys.size() < nrFrontals) throw invalid_argument(
"EliminateSymbolic requested to eliminate more variables than exist in graph.");
vector<Index> newKeys(keys.begin(), keys.end());
return make_pair(boost::make_shared<IndexConditionalOrdered>(newKeys, nrFrontals),
boost::make_shared<IndexFactorOrdered>(newKeys.begin() + nrFrontals, newKeys.end()));
}
/* ************************************************************************* */
}

72
gtsam/inference/SymbolicFactorGraph.h → gtsam/inference/SymbolicFactorGraphOrdered.h
View File

@ -18,24 +18,24 @@
#pragma once #pragma once
#include <gtsam/global_includes.h> #include <gtsam/global_includes.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraphOrdered.h>
#include <gtsam/inference/IndexFactor.h> #include <gtsam/inference/IndexFactorOrdered.h>
namespace gtsam { template<class FACTOR> class EliminationTree; } namespace gtsam { template<class FACTOR> class EliminationTreeOrdered; }
namespace gtsam { template<class CONDITIONAL> class BayesNet; } namespace gtsam { template<class CONDITIONAL> class BayesNetOrdered; }
namespace gtsam { template<class CONDITIONAL, class CLIQUE> class BayesTree; } namespace gtsam { template<class CONDITIONAL, class CLIQUE> class BayesTreeOrdered; }
namespace gtsam { class IndexConditional; } namespace gtsam { class IndexConditionalOrdered; }
namespace gtsam { namespace gtsam {
typedef EliminationTree<IndexFactor> SymbolicEliminationTree; typedef EliminationTreeOrdered<IndexFactorOrdered> SymbolicEliminationTreeOrdered;
typedef BayesNet<IndexConditional> SymbolicBayesNet; typedef BayesNetOrdered<IndexConditionalOrdered> SymbolicBayesNetOrdered;
typedef BayesTree<IndexConditional> SymbolicBayesTree; typedef BayesTreeOrdered<IndexConditionalOrdered> SymbolicBayesTreeOrdered;
/** Symbolic IndexFactor Graph /** Symbolic IndexFactor Graph
* \nosubgrouping * \nosubgrouping
*/ */
class SymbolicFactorGraph: public FactorGraph<IndexFactor> { class SymbolicFactorGraphOrdered: public FactorGraphOrdered<IndexFactorOrdered> {
public: public:
@ -43,29 +43,29 @@ namespace gtsam {
/// @{ /// @{
/** Construct empty factor graph */ /** Construct empty factor graph */
SymbolicFactorGraph() { SymbolicFactorGraphOrdered() {
} }
/** Construct from a BayesNet */ /** Construct from a BayesNet */
GTSAM_EXPORT SymbolicFactorGraph(const SymbolicBayesNet& bayesNet); GTSAM_EXPORT SymbolicFactorGraphOrdered(const SymbolicBayesNetOrdered& bayesNet);
/** Construct from a BayesTree */ /** Construct from a BayesTree */
GTSAM_EXPORT SymbolicFactorGraph(const SymbolicBayesTree& bayesTree); GTSAM_EXPORT SymbolicFactorGraphOrdered(const SymbolicBayesTreeOrdered& bayesTree);
/** /**
* Construct from a factor graph of any type * Construct from a factor graph of any type
*/ */
template<class FACTOR> template<class FACTOR>
SymbolicFactorGraph(const FactorGraph<FACTOR>& fg); SymbolicFactorGraphOrdered(const FactorGraphOrdered<FACTOR>& fg);
/** Eliminate the first \c n frontal variables, returning the resulting /** Eliminate the first \c n frontal variables, returning the resulting
* conditional and remaining factor graph - this is very inefficient for * conditional and remaining factor graph - this is very inefficient for
* eliminating all variables, to do that use EliminationTree or * eliminating all variables, to do that use EliminationTree or
* JunctionTree. Note that this version simply calls * JunctionTree. Note that this version simply calls
* FactorGraph<IndexFactor>::eliminateFrontals with EliminateSymbolic * FactorGraph<IndexFactor>::eliminateFrontals with EliminateSymbolic
* as the eliminate function argument. * as the eliminate function argument.
*/ */
GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> eliminateFrontals(size_t nFrontals) const; GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraphOrdered> eliminateFrontals(size_t nFrontals) const;
/** Factor the factor graph into a conditional and a remaining factor graph. /** Factor the factor graph into a conditional and a remaining factor graph.
* Given the factor graph \f$ f(X) \f$, and \c variables to factorize out * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out
@ -79,14 +79,14 @@ namespace gtsam {
* conditional on all of the variables in \c variables, instead of a sparse * conditional on all of the variables in \c variables, instead of a sparse
* BayesNet. If the variables are not fully-connected, it is more efficient * BayesNet. If the variables are not fully-connected, it is more efficient
* to sequentially factorize multiple times. * to sequentially factorize multiple times.
* Note that this version simply calls * Note that this version simply calls
* FactorGraph<GaussianFactor>::eliminate with EliminateSymbolic as the eliminate * FactorGraph<GaussianFactor>::eliminate with EliminateSymbolic as the eliminate
* function argument. * function argument.
*/ */
GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> eliminate(const std::vector<Index>& variables) const; GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraphOrdered> eliminate(const std::vector<Index>& variables) const;
/** Eliminate a single variable, by calling SymbolicFactorGraph::eliminate. */ /** Eliminate a single variable, by calling SymbolicFactorGraph::eliminate. */
GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> eliminateOne(Index variable) const; GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraphOrdered> eliminateOne(Index variable) const;
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
@ -113,17 +113,17 @@ namespace gtsam {
/** Push back 4-way factor */ /** Push back 4-way factor */
GTSAM_EXPORT void push_factor(Index key1, Index key2, Index key3, Index key4); GTSAM_EXPORT void push_factor(Index key1, Index key2, Index key3, Index key4);
/** Permute the variables in the factors */ /** Permute the variables in the factors */
GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation); GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation);
/** Apply a reduction, which is a remapping of variable indices. */ /** Apply a reduction, which is a remapping of variable indices. */
GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction); GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction);
}; };
/** Create a combined joint factor (new style for EliminationTree). */ /** Create a combined joint factor (new style for EliminationTree). */
GTSAM_EXPORT IndexFactor::shared_ptr CombineSymbolic(const FactorGraph<IndexFactor>& factors, GTSAM_EXPORT IndexFactorOrdered::shared_ptr CombineSymbolic(const FactorGraphOrdered<IndexFactorOrdered>& factors,
const FastMap<Index, std::vector<Index> >& variableSlots); const FastMap<Index, std::vector<Index> >& variableSlots);
/** /**
@ -131,20 +131,20 @@ namespace gtsam {
* Combine and eliminate can also be called separately, but for this and * Combine and eliminate can also be called separately, but for this and
* derived classes calling them separately generally does extra work. * derived classes calling them separately generally does extra work.
*/ */
GTSAM_EXPORT std::pair<boost::shared_ptr<IndexConditional>, boost::shared_ptr<IndexFactor> > GTSAM_EXPORT std::pair<boost::shared_ptr<IndexConditionalOrdered>, boost::shared_ptr<IndexFactorOrdered> >
EliminateSymbolic(const FactorGraph<IndexFactor>&, size_t nrFrontals = 1); EliminateSymbolic(const FactorGraphOrdered<IndexFactorOrdered>&, size_t nrFrontals = 1);
/// @} /// @}
/** Template function implementation */ /** Template function implementation */
template<class FACTOR> template<class FACTOR>
SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph<FACTOR>& fg) { SymbolicFactorGraphOrdered::SymbolicFactorGraphOrdered(const FactorGraphOrdered<FACTOR>& fg) {
for (size_t i = 0; i < fg.size(); i++) { for (size_t i = 0; i < fg.size(); i++) {
if (fg[i]) { if (fg[i]) {
IndexFactor::shared_ptr factor(new IndexFactor(*fg[i])); IndexFactorOrdered::shared_ptr factor(new IndexFactorOrdered(*fg[i]));
push_back(factor); push_back(factor);
} else } else
push_back(IndexFactor::shared_ptr()); push_back(IndexFactorOrdered::shared_ptr());
} }
} }

4
gtsam/inference/SymbolicMultifrontalSolver.cpp → gtsam/inference/SymbolicMultifrontalSolverOrdered.cpp
View File

@ -15,8 +15,8 @@
* @date Oct 22, 2010 * @date Oct 22, 2010
*/ */
#include <gtsam/inference/SymbolicMultifrontalSolver.h> #include <gtsam/inference/SymbolicMultifrontalSolverOrdered.h>
#include <gtsam/inference/JunctionTree.h> #include <gtsam/inference/JunctionTreeOrdered.h>
namespace gtsam { namespace gtsam {

18
gtsam/inference/SymbolicMultifrontalSolver.h → gtsam/inference/SymbolicMultifrontalSolverOrdered.h
View File

@ -18,14 +18,14 @@
#pragma once #pragma once
#include <gtsam/inference/GenericMultifrontalSolver.h> #include <gtsam/inference/GenericMultifrontalSolver.h>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraphOrdered.h>
namespace gtsam { namespace gtsam {
class SymbolicMultifrontalSolver : GenericMultifrontalSolver<IndexFactor, JunctionTree<FactorGraph<IndexFactor> > > { class SymbolicMultifrontalSolver : GenericMultifrontalSolver<IndexFactorOrdered, JunctionTreeOrdered<FactorGraphOrdered<IndexFactorOrdered> > > {
protected: protected:
typedef GenericMultifrontalSolver<IndexFactor, JunctionTree<FactorGraph<IndexFactor> > > Base; typedef GenericMultifrontalSolver<IndexFactorOrdered, JunctionTreeOrdered<FactorGraphOrdered<IndexFactorOrdered> > > Base;
public: public:
/** /**
@ -33,15 +33,15 @@ namespace gtsam {
* tree, which does the symbolic elimination, identifies the cliques, * tree, which does the symbolic elimination, identifies the cliques,
* and distributes all the factors to the right cliques. * and distributes all the factors to the right cliques.
*/ */
SymbolicMultifrontalSolver(const SymbolicFactorGraph& factorGraph) : Base(factorGraph) {}; SymbolicMultifrontalSolver(const SymbolicFactorGraphOrdered& factorGraph) : Base(factorGraph) {};
/** /**
* Construct the solver with a shared pointer to a factor graph and to a * Construct the solver with a shared pointer to a factor graph and to a
* VariableIndex. The solver will store these pointers, so this constructor * VariableIndex. The solver will store these pointers, so this constructor
* is the fastest. * is the fastest.
*/ */
SymbolicMultifrontalSolver(const SymbolicFactorGraph::shared_ptr& factorGraph, SymbolicMultifrontalSolver(const SymbolicFactorGraphOrdered::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {}; const VariableIndexOrdered::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {};
/** Print to cout */ /** Print to cout */
void print(const std::string& name = "SymbolicMultifrontalSolver: ") const { Base::print(name); }; void print(const std::string& name = "SymbolicMultifrontalSolver: ") const { Base::print(name); };
@ -53,19 +53,19 @@ namespace gtsam {
* Eliminate the factor graph sequentially. Uses a column elimination tree * Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate. * to recursively eliminate.
*/ */
SymbolicBayesTree::shared_ptr eliminate() const { return Base::eliminate(&EliminateSymbolic); }; SymbolicBayesTreeOrdered::shared_ptr eliminate() const { return Base::eliminate(&EliminateSymbolic); };
/** /**
* Compute the marginal joint over a set of variables, by integrating out * Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. Returns the result as a factor graph. * all of the other variables. Returns the result as a factor graph.
*/ */
SymbolicFactorGraph::shared_ptr jointFactorGraph(const std::vector<Index>& js) const { return Base::jointFactorGraph(js, &EliminateSymbolic); }; SymbolicFactorGraphOrdered::shared_ptr jointFactorGraph(const std::vector<Index>& js) const { return Base::jointFactorGraph(js, &EliminateSymbolic); };
/** /**
* Compute the marginal Gaussian density over a variable, by integrating out * Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as a factor. * all of the other variables. This function returns the result as a factor.
*/ */
IndexFactor::shared_ptr marginalFactor(Index j) const { return Base::marginalFactor(j, &EliminateSymbolic); }; IndexFactorOrdered::shared_ptr marginalFactor(Index j) const { return Base::marginalFactor(j, &EliminateSymbolic); };
}; };
} }

2
gtsam/inference/SymbolicSequentialSolver.cpp → gtsam/inference/SymbolicSequentialSolverOrdered.cpp
View File

@ -15,7 +15,7 @@
* @date Oct 21, 2010 * @date Oct 21, 2010
*/ */
#include <gtsam/inference/SymbolicSequentialSolver.h> #include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
#include <gtsam/inference/GenericSequentialSolver-inl.h> #include <gtsam/inference/GenericSequentialSolver-inl.h>
namespace gtsam { namespace gtsam {

22
gtsam/inference/SymbolicSequentialSolver.h → gtsam/inference/SymbolicSequentialSolverOrdered.h
View File

@ -17,14 +17,14 @@
#pragma once #pragma once
#include <gtsam/inference/GenericSequentialSolver.h> #include <gtsam/inference/GenericSequentialSolver.h>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraphOrdered.h>
namespace gtsam { namespace gtsam {
class SymbolicSequentialSolver : GenericSequentialSolver<IndexFactor> { class SymbolicSequentialSolver : GenericSequentialSolver<IndexFactorOrdered> {
protected: protected:
typedef GenericSequentialSolver<IndexFactor> Base; typedef GenericSequentialSolver<IndexFactorOrdered> Base;
public: public:
/** /**
@ -32,15 +32,15 @@ namespace gtsam {
* tree, which does the symbolic elimination, identifies the cliques, * tree, which does the symbolic elimination, identifies the cliques,
* and distributes all the factors to the right cliques. * and distributes all the factors to the right cliques.
*/ */
SymbolicSequentialSolver(const SymbolicFactorGraph& factorGraph) : Base(factorGraph) {}; SymbolicSequentialSolver(const SymbolicFactorGraphOrdered& factorGraph) : Base(factorGraph) {};
/** /**
* Construct the solver with a shared pointer to a factor graph and to a * Construct the solver with a shared pointer to a factor graph and to a
* VariableIndex. The solver will store these pointers, so this constructor * VariableIndex. The solver will store these pointers, so this constructor
* is the fastest. * is the fastest.
*/ */
SymbolicSequentialSolver(const SymbolicFactorGraph::shared_ptr& factorGraph, SymbolicSequentialSolver(const SymbolicFactorGraphOrdered::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {}; const VariableIndexOrdered::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {};
/** Print to cout */ /** Print to cout */
void print(const std::string& name = "SymbolicSequentialSolver: ") const { Base::print(name); }; void print(const std::string& name = "SymbolicSequentialSolver: ") const { Base::print(name); };
@ -52,7 +52,7 @@ namespace gtsam {
* Eliminate the factor graph sequentially. Uses a column elimination tree * Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate. * to recursively eliminate.
*/ */
SymbolicBayesNet::shared_ptr eliminate() const SymbolicBayesNetOrdered::shared_ptr eliminate() const
{ return Base::eliminate(&EliminateSymbolic); }; { return Base::eliminate(&EliminateSymbolic); };
/** /**
@ -60,28 +60,28 @@ namespace gtsam {
* P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S) * P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S)
* Returns the result as a Bayes net. * Returns the result as a Bayes net.
*/ */
SymbolicBayesNet::shared_ptr conditionalBayesNet(const std::vector<Index>& js, size_t nrFrontals) const SymbolicBayesNetOrdered::shared_ptr conditionalBayesNet(const std::vector<Index>& js, size_t nrFrontals) const
{ return Base::conditionalBayesNet(js, nrFrontals, &EliminateSymbolic); }; { return Base::conditionalBayesNet(js, nrFrontals, &EliminateSymbolic); };
/** /**
* Compute the marginal joint over a set of variables, by integrating out * Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. Returns the result as a Bayes net. * all of the other variables. Returns the result as a Bayes net.
*/ */
SymbolicBayesNet::shared_ptr jointBayesNet(const std::vector<Index>& js) const SymbolicBayesNetOrdered::shared_ptr jointBayesNet(const std::vector<Index>& js) const
{ return Base::jointBayesNet(js, &EliminateSymbolic); }; { return Base::jointBayesNet(js, &EliminateSymbolic); };
/** /**
* Compute the marginal joint over a set of variables, by integrating out * Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. Returns the result as a factor graph. * all of the other variables. Returns the result as a factor graph.
*/ */
SymbolicFactorGraph::shared_ptr jointFactorGraph(const std::vector<Index>& js) const SymbolicFactorGraphOrdered::shared_ptr jointFactorGraph(const std::vector<Index>& js) const
{ return Base::jointFactorGraph(js, &EliminateSymbolic); }; { return Base::jointFactorGraph(js, &EliminateSymbolic); };
/** /**
* Compute the marginal Gaussian density over a variable, by integrating out * Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as a factor. * all of the other variables. This function returns the result as a factor.
*/ */
IndexFactor::shared_ptr marginalFactor(Index j) const { return Base::marginalFactor(j, &EliminateSymbolic); }; IndexFactorOrdered::shared_ptr marginalFactor(Index j) const { return Base::marginalFactor(j, &EliminateSymbolic); };
}; };
} }

8
gtsam/inference/VariableIndexUnordered-inl.h → gtsam/inference/VariableIndex-inl.h
View File

@ -17,13 +17,13 @@
#pragma once #pragma once
#include <gtsam/inference/VariableIndexUnordered.h> #include <gtsam/inference/VariableIndex.h>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG> template<class FG>
void VariableIndexUnordered::augment(const FG& factors) void VariableIndex::augment(const FG& factors)
{ {
gttic(VariableIndex_augment); gttic(VariableIndex_augment);
@ -45,7 +45,7 @@ void VariableIndexUnordered::augment(const FG& factors)
/* ************************************************************************* */ /* ************************************************************************* */
template<typename ITERATOR, class FG> template<typename ITERATOR, class FG>
void VariableIndexUnordered::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors) void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors)
{ {
gttic(VariableIndex_remove); gttic(VariableIndex_remove);
@ -73,7 +73,7 @@ void VariableIndexUnordered::remove(ITERATOR firstFactor, ITERATOR lastFactor, c
/* ************************************************************************* */ /* ************************************************************************* */
template<typename ITERATOR> template<typename ITERATOR>
void VariableIndexUnordered::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) { void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) {
for(ITERATOR key = firstKey; key != lastKey; ++key) { for(ITERATOR key = firstKey; key != lastKey; ++key) {
assert(internalAt(*key).empty()); assert(internalAt(*key).empty());
index_.erase(*key); index_.erase(*key);

162
gtsam/inference/VariableIndex.cpp
View File

@ -1,104 +1,58 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file VariableIndex.cpp * @file VariableIndex.cpp
* @author Richard Roberts * @author Richard Roberts
* @date Oct 22, 2010 * @date March 26, 2013
*/ */
#include <iostream> #include <iostream>
#include <gtsam/inference/VariableIndex.h> #include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/Permutation.h>
namespace gtsam {
namespace gtsam {
using namespace std;
using namespace std;
/* ************************************************************************* */
/* ************************************************************************* */ bool VariableIndex::equals(const VariableIndex& other, double tol) const {
bool VariableIndex::equals(const VariableIndex& other, double tol) const { return this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_
if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) { && this->index_ == other.index_;
for(size_t var=0; var < max(this->index_.size(), other.index_.size()); ++var) }
if(var >= this->index_.size() || var >= other.index_.size()) {
if(!((var >= this->index_.size() && other.index_[var].empty()) || /* ************************************************************************* */
(var >= other.index_.size() && this->index_[var].empty()))) void VariableIndex::print(const string& str) const {
return false; cout << str;
} else if(this->index_[var] != other.index_[var]) cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
return false; BOOST_FOREACH(KeyMap::value_type key_factors, index_) {
} else cout << "var " << key_factors.first << ":";
return false; BOOST_FOREACH(const size_t factor, key_factors.second)
return true; cout << " " << factor;
} cout << "\n";
}
/* ************************************************************************* */ cout.flush();
void VariableIndex::print(const string& str) const { }
cout << str;
cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n"; /* ************************************************************************* */
for(Index var = 0; var < size(); ++var) { void VariableIndex::outputMetisFormat(ostream& os) const {
cout << "var " << var << ":"; os << size() << " " << nFactors() << "\n";
BOOST_FOREACH(const size_t factor, index_[var]) // run over variables, which will be hyper-edges.
cout << " " << factor; BOOST_FOREACH(KeyMap::value_type key_factors, index_) {
cout << "\n"; // every variable is a hyper-edge covering its factors
} BOOST_FOREACH(const size_t factor, key_factors.second)
cout << flush; os << (factor+1) << " "; // base 1
} os << "\n";
}
/* ************************************************************************* */ os << flush;
void VariableIndex::outputMetisFormat(ostream& os) const { }
os << size() << " " << nFactors() << "\n";
// run over variables, which will be hyper-edges. }
BOOST_FOREACH(const Factors& variable, index_) {
// every variable is a hyper-edge covering its factors
BOOST_FOREACH(const size_t factor, variable)
os << (factor+1) << " "; // base 1
os << "\n";
}
os << flush;
}
/* ************************************************************************* */
void VariableIndex::permuteInPlace(const Permutation& permutation) {
// Create new index and move references to data into it in permuted order
vector<VariableIndex::Factors> newIndex(this->size());
for(Index i = 0; i < newIndex.size(); ++i)
newIndex[i].swap(this->index_[permutation[i]]);
// Move reference to entire index into the VariableIndex
index_.swap(newIndex);
}
/* ************************************************************************* */
void VariableIndex::permuteInPlace(const Permutation& selector, const Permutation& permutation) {
if(selector.size() != permutation.size())
throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes.");
// Create new index the size of the permuted entries
vector<VariableIndex::Factors> newIndex(selector.size());
// Permute the affected entries into the new index
for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
newIndex[dstSlot].swap(this->index_[selector[permutation[dstSlot]]]);
// Put the affected entries back in the new order
for(size_t slot = 0; slot < selector.size(); ++slot)
this->index_[selector[slot]].swap(newIndex[slot]);
}
/* ************************************************************************* */
void VariableIndex::removeUnusedAtEnd(size_t nToRemove) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
for(size_t i = this->size() - nToRemove; i < this->size(); ++i)
if(!(*this)[i].empty())
throw std::invalid_argument("Attempting to remove non-empty variables with VariableIndex::removeUnusedAtEnd()");
#endif
index_.resize(this->size() - nToRemove);
}
}

474
gtsam/inference/VariableIndex.h
View File

@ -1,292 +1,182 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file VariableIndex.h * @file VariableIndex.h
* @author Richard Roberts * @author Richard Roberts
* @date Sep 12, 2010 * @date March 26, 2013
*/ */
#pragma once #pragma once
#include <vector> #include <vector>
#include <deque> #include <deque>
#include <stdexcept> #include <stdexcept>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
#include <gtsam/global_includes.h> #include <gtsam/base/FastMap.h>
#include <gtsam/base/timing.h> #include <gtsam/base/types.h>
#include <gtsam/base/timing.h>
namespace gtsam { #include <gtsam/inference/Key.h>
class Permutation; namespace gtsam {
/** /**
* The VariableIndex class computes and stores the block column structure of a * The VariableIndex class computes and stores the block column structure of a
* factor graph. The factor graph stores a collection of factors, each of * factor graph. The factor graph stores a collection of factors, each of
* which involves a set of variables. In contrast, the VariableIndex is built * which involves a set of variables. In contrast, the VariableIndex is built
* from a factor graph prior to elimination, and stores the list of factors * from a factor graph prior to elimination, and stores the list of factors
* that involve each variable. This information is stored as a deque of * that involve each variable. This information is stored as a deque of
* lists of factor indices. * lists of factor indices.
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT VariableIndex { class GTSAM_EXPORT VariableIndex {
public: public:
typedef boost::shared_ptr<VariableIndex> shared_ptr; typedef boost::shared_ptr<VariableIndex> shared_ptr;
typedef FastList<size_t> Factors; typedef FastList<size_t> Factors;
typedef Factors::iterator Factor_iterator; typedef Factors::iterator Factor_iterator;
typedef Factors::const_iterator Factor_const_iterator; typedef Factors::const_iterator Factor_const_iterator;
protected: protected:
std::vector<Factors> index_; typedef FastMap<Key,Factors> KeyMap;
size_t nFactors_; // Number of factors in the original factor graph. KeyMap index_;
size_t nEntries_; // Sum of involved variable counts of each factor. size_t nFactors_; // Number of factors in the original factor graph.
size_t nEntries_; // Sum of involved variable counts of each factor.
public:
public:
/// @name Standard Constructors typedef KeyMap::const_iterator const_iterator;
/// @{ typedef KeyMap::const_iterator iterator;
typedef KeyMap::value_type value_type;
/** Default constructor, creates an empty VariableIndex */
VariableIndex() : nFactors_(0), nEntries_(0) {} public:
/** /// @name Standard Constructors
* Create a VariableIndex that computes and stores the block column structure /// @{
* of a factor graph. This constructor is used when the number of variables
* is known beforehand. /** Default constructor, creates an empty VariableIndex */
*/ VariableIndex() : nFactors_(0), nEntries_(0) {}
template<class FG> VariableIndex(const FG& factorGraph, Index nVariables);
/**
/** * Create a VariableIndex that computes and stores the block column structure
* Create a VariableIndex that computes and stores the block column structure * of a factor graph.
* of a factor graph. */
*/ template<class FG>
template<class FG> VariableIndex(const FG& factorGraph); VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) { augment(factorGraph); }
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/** /**
* The number of variable entries. This is one greater than the variable * The number of variable entries. This is one greater than the variable
* with the highest index. * with the highest index.
*/ */
Index size() const { return index_.size(); } Index size() const { return index_.size(); }
/** The number of factors in the original factor graph */ /** The number of factors in the original factor graph */
size_t nFactors() const { return nFactors_; } size_t nFactors() const { return nFactors_; }
/** The number of nonzero blocks, i.e. the number of variable-factor entries */ /** The number of nonzero blocks, i.e. the number of variable-factor entries */
size_t nEntries() const { return nEntries_; } size_t nEntries() const { return nEntries_; }
/** Access a list of factors by variable */ /** Access a list of factors by variable */
const Factors& operator[](Index variable) const { checkVar(variable); return index_[variable]; } const Factors& operator[](Key variable) const {
KeyMap::const_iterator item = index_.find(variable);
/// @} if(item == index_.end())
/// @name Testable throw std::invalid_argument("Requested non-existent variable from VariableIndex");
/// @{ else
return item->second;
/** Test for equality (for unit tests and debug assertions). */ }
bool equals(const VariableIndex& other, double tol=0.0) const;
/// @}
/** Print the variable index (for unit tests and debugging). */ /// @name Testable
void print(const std::string& str = "VariableIndex: ") const; /// @{
/** /** Test for equality (for unit tests and debug assertions). */
* Output dual hypergraph to Metis file format for use with hmetis bool equals(const VariableIndex& other, double tol=0.0) const;
* In the dual graph, variables are hyperedges, factors are nodes.
*/ /** Print the variable index (for unit tests and debugging). */
void outputMetisFormat(std::ostream& os) const; void print(const std::string& str = "VariableIndex: ") const;
/**
/// @} * Output dual hypergraph to Metis file format for use with hmetis
/// @name Advanced Interface * In the dual graph, variables are hyperedges, factors are nodes.
/// @{ */
void outputMetisFormat(std::ostream& os) const;
/**
* Augment the variable index with new factors. This can be used when
* solving problems incrementally. /// @}
*/ /// @name Advanced Interface
template<class FG> void augment(const FG& factors); /// @{
/** /**
* Remove entries corresponding to the specified factors. * Augment the variable index with new factors. This can be used when
* NOTE: We intentionally do not decrement nFactors_ because the factor * solving problems incrementally.
* indices need to remain consistent. Removing factors from a factor graph */
* does not shift the indices of other factors. Also, we keep nFactors_ template<class FG>
* one greater than the highest-numbered factor referenced in a VariableIndex. void augment(const FG& factors);
*
* @param indices The indices of the factors to remove, which must match \c factors /**
* @param factors The factors being removed, which must symbolically correspond * Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement
* exactly to the factors with the specified \c indices that were added. * nFactors_ because the factor indices need to remain consistent. Removing factors from a factor
*/ * graph does not shift the indices of other factors. Also, we keep nFactors_ one greater than
template<typename CONTAINER, class FG> void remove(const CONTAINER& indices, const FG& factors); * the highest-numbered factor referenced in a VariableIndex.
*
/// Permute the variables in the VariableIndex according to the given permutation * @param indices The indices of the factors to remove, which must match \c factors
void permuteInPlace(const Permutation& permutation); * @param factors The factors being removed, which must symbolically correspond exactly to the
* factors with the specified \c indices that were added.
/// Permute the variables in the VariableIndex according to the given partial permutation */
void permuteInPlace(const Permutation& selector, const Permutation& permutation); template<typename ITERATOR, class FG>
void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors);
/** Remove unused empty variables at the end of the ordering (in debug mode
* verifies they are empty). /** Remove unused empty variables at the end of the ordering (in debug mode
* @param nToRemove The number of unused variables at the end to remove * verifies they are empty).
*/ * @param nToRemove The number of unused variables at the end to remove
void removeUnusedAtEnd(size_t nToRemove); */
template<typename ITERATOR>
protected: void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey);
Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); }
Factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); } /** Iterator to the first variable entry */
const_iterator begin() const { return index_.begin(); }
Factor_const_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); }
Factor_const_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); } /** Iterator to the first variable entry */
const_iterator end() const { return index_.end(); }
/// Internal constructor to allocate a VariableIndex of the requested size
VariableIndex(size_t nVars) : index_(nVars), nFactors_(0), nEntries_(0) {} /** Find the iterator for the requested variable entry */
const_iterator find(Key key) const { return index_.find(key); }
/// Internal check of the validity of a variable
void checkVar(Index variable) const { assert(variable < index_.size()); } protected:
Factor_iterator factorsBegin(Index variable) { return internalAt(variable).begin(); }
/// Internal function to populate the variable index from a factor graph Factor_iterator factorsEnd(Index variable) { return internalAt(variable).end(); }
template<class FG> void fill(const FG& factorGraph);
Factor_const_iterator factorsBegin(Index variable) const { return internalAt(variable).begin(); }
/// @} Factor_const_iterator factorsEnd(Index variable) const { return internalAt(variable).end(); }
private: /// Internal version of 'at' that asserts existence
/** Serialization function */ const Factors& internalAt(Key variable) const {
friend class boost::serialization::access; const KeyMap::const_iterator item = index_.find(variable);
template<class ARCHIVE> assert(item != index_.end());
void serialize(ARCHIVE & ar, const unsigned int version) { return item->second; }
ar & BOOST_SERIALIZATION_NVP(index_);
ar & BOOST_SERIALIZATION_NVP(nFactors_); /// Internal version of 'at' that asserts existence
ar & BOOST_SERIALIZATION_NVP(nEntries_); Factors& internalAt(Key variable) {
} const KeyMap::iterator item = index_.find(variable);
}; assert(item != index_.end());
return item->second; }
/* ************************************************************************* */
template<class FG> /// @}
void VariableIndex::fill(const FG& factorGraph) { };
gttic(VariableIndex_fill);
// Build index mapping from variable id to factor index }
for(size_t fi=0; fi<factorGraph.size(); ++fi) {
if(factorGraph[fi]) { #include <gtsam/inference/VariableIndex-inl.h>
BOOST_FOREACH(const Index key, factorGraph[fi]->keys()) {
if(key < index_.size()) {
index_[key].push_back(fi);
++ nEntries_;
}
}
}
++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent
}
}
/* ************************************************************************* */
template<class FG>
VariableIndex::VariableIndex(const FG& factorGraph) :
nFactors_(0), nEntries_(0)
{
gttic(VariableIndex_constructor);
// If the factor graph is empty, return an empty index because inside this
// if block we assume at least one factor.
if(factorGraph.size() > 0) {
gttic(VariableIndex_constructor_find_highest);
// Find highest-numbered variable
Index maxVar = 0;
BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) {
if(factor) {
BOOST_FOREACH(const Index key, factor->keys()) {
if(key > maxVar)
maxVar = key;
}
}
}
gttoc(VariableIndex_constructor_find_highest);
// Allocate array
gttic(VariableIndex_constructor_allocate);
index_.resize(maxVar+1);
gttoc(VariableIndex_constructor_allocate);
fill(factorGraph);
}
}
/* ************************************************************************* */
template<class FG>
VariableIndex::VariableIndex(const FG& factorGraph, Index nVariables) :
nFactors_(0), nEntries_(0)
{
gttic(VariableIndex_constructor_allocate);
index_.resize(nVariables);
gttoc(VariableIndex_constructor_allocate);
fill(factorGraph);
}
/* ************************************************************************* */
template<class FG>
void VariableIndex::augment(const FG& factors) {
gttic(VariableIndex_augment);
// If the factor graph is empty, return an empty index because inside this
// if block we assume at least one factor.
if(factors.size() > 0) {
// Find highest-numbered variable
Index maxVar = 0;
BOOST_FOREACH(const typename FG::sharedFactor& factor, factors) {
if(factor) {
BOOST_FOREACH(const Index key, factor->keys()) {
if(key > maxVar)
maxVar = key;
}
}
}
// Allocate index
index_.resize(std::max(index_.size(), maxVar+1));
// Augment index mapping from variable id to factor index
size_t orignFactors = nFactors_;
for(size_t fi=0; fi<factors.size(); ++fi) {
if(factors[fi]) {
BOOST_FOREACH(const Index key, factors[fi]->keys()) {
index_[key].push_back(orignFactors + fi);
++ nEntries_;
}
}
++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent
}
}
}
/* ************************************************************************* */
template<typename CONTAINER, class FG>
void VariableIndex::remove(const CONTAINER& indices, const FG& factors) {
gttic(VariableIndex_remove);
// NOTE: We intentionally do not decrement nFactors_ because the factor
// indices need to remain consistent. Removing factors from a factor graph
// does not shift the indices of other factors. Also, we keep nFactors_
// one greater than the highest-numbered factor referenced in a VariableIndex.
for(size_t fi=0; fi<factors.size(); ++fi)
if(factors[fi]) {
for(size_t ji = 0; ji < factors[fi]->keys().size(); ++ji) {
Factors& factorEntries = index_[factors[fi]->keys()[ji]];
Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), indices[fi]);
if(entry == factorEntries.end())
throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
factorEntries.erase(entry);
-- nEntries_;
}
}
}
}

104
gtsam/inference/VariableIndexOrdered.cpp Normal file
View File

@ -0,0 +1,104 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file VariableIndex.cpp
* @author Richard Roberts
* @date Oct 22, 2010
*/
#include <iostream>
#include <gtsam/inference/VariableIndexOrdered.h>
#include <gtsam/inference/PermutationOrdered.h>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
bool VariableIndexOrdered::equals(const VariableIndexOrdered& other, double tol) const {
if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) {
for(size_t var=0; var < max(this->index_.size(), other.index_.size()); ++var)
if(var >= this->index_.size() || var >= other.index_.size()) {
if(!((var >= this->index_.size() && other.index_[var].empty()) ||
(var >= other.index_.size() && this->index_[var].empty())))
return false;
} else if(this->index_[var] != other.index_[var])
return false;
} else
return false;
return true;
}
/* ************************************************************************* */
void VariableIndexOrdered::print(const string& str) const {
cout << str;
cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
for(Index var = 0; var < size(); ++var) {
cout << "var " << var << ":";
BOOST_FOREACH(const size_t factor, index_[var])
cout << " " << factor;
cout << "\n";
}
cout << flush;
}
/* ************************************************************************* */
void VariableIndexOrdered::outputMetisFormat(ostream& os) const {
os << size() << " " << nFactors() << "\n";
// run over variables, which will be hyper-edges.
BOOST_FOREACH(const Factors& variable, index_) {
// every variable is a hyper-edge covering its factors
BOOST_FOREACH(const size_t factor, variable)
os << (factor+1) << " "; // base 1
os << "\n";
}
os << flush;
}
/* ************************************************************************* */
void VariableIndexOrdered::permuteInPlace(const Permutation& permutation) {
// Create new index and move references to data into it in permuted order
vector<VariableIndexOrdered::Factors> newIndex(this->size());
for(Index i = 0; i < newIndex.size(); ++i)
newIndex[i].swap(this->index_[permutation[i]]);
// Move reference to entire index into the VariableIndex
index_.swap(newIndex);
}
/* ************************************************************************* */
void VariableIndexOrdered::permuteInPlace(const Permutation& selector, const Permutation& permutation) {
if(selector.size() != permutation.size())
throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes.");
// Create new index the size of the permuted entries
vector<VariableIndexOrdered::Factors> newIndex(selector.size());
// Permute the affected entries into the new index
for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
newIndex[dstSlot].swap(this->index_[selector[permutation[dstSlot]]]);
// Put the affected entries back in the new order
for(size_t slot = 0; slot < selector.size(); ++slot)
this->index_[selector[slot]].swap(newIndex[slot]);
}
/* ************************************************************************* */
void VariableIndexOrdered::removeUnusedAtEnd(size_t nToRemove) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
for(size_t i = this->size() - nToRemove; i < this->size(); ++i)
if(!(*this)[i].empty())
throw std::invalid_argument("Attempting to remove non-empty variables with VariableIndex::removeUnusedAtEnd()");
#endif
index_.resize(this->size() - nToRemove);
}
}

292
gtsam/inference/VariableIndexOrdered.h Normal file
View File

@ -0,0 +1,292 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file VariableIndex.h
* @author Richard Roberts
* @date Sep 12, 2010
*/
#pragma once
#include <vector>
#include <deque>
#include <stdexcept>
#include <boost/foreach.hpp>
#include <gtsam/base/FastList.h>
#include <gtsam/global_includes.h>
#include <gtsam/base/timing.h>
namespace gtsam {
class Permutation;
/**
* The VariableIndex class computes and stores the block column structure of a
* factor graph. The factor graph stores a collection of factors, each of
* which involves a set of variables. In contrast, the VariableIndex is built
* from a factor graph prior to elimination, and stores the list of factors
* that involve each variable. This information is stored as a deque of
* lists of factor indices.
* \nosubgrouping
*/
class GTSAM_EXPORT VariableIndexOrdered {
public:
typedef boost::shared_ptr<VariableIndexOrdered> shared_ptr;
typedef FastList<size_t> Factors;
typedef Factors::iterator Factor_iterator;
typedef Factors::const_iterator Factor_const_iterator;
protected:
std::vector<Factors> index_;
size_t nFactors_; // Number of factors in the original factor graph.
size_t nEntries_; // Sum of involved variable counts of each factor.
public:
/// @name Standard Constructors
/// @{
/** Default constructor, creates an empty VariableIndex */
VariableIndexOrdered() : nFactors_(0), nEntries_(0) {}
/**
* Create a VariableIndex that computes and stores the block column structure
* of a factor graph. This constructor is used when the number of variables
* is known beforehand.
*/
template<class FG> VariableIndexOrdered(const FG& factorGraph, Index nVariables);
/**
* Create a VariableIndex that computes and stores the block column structure
* of a factor graph.
*/
template<class FG> VariableIndexOrdered(const FG& factorGraph);
/// @}
/// @name Standard Interface
/// @{
/**
* The number of variable entries. This is one greater than the variable
* with the highest index.
*/
Index size() const { return index_.size(); }
/** The number of factors in the original factor graph */
size_t nFactors() const { return nFactors_; }
/** The number of nonzero blocks, i.e. the number of variable-factor entries */
size_t nEntries() const { return nEntries_; }
/** Access a list of factors by variable */
const Factors& operator[](Index variable) const { checkVar(variable); return index_[variable]; }
/// @}
/// @name Testable
/// @{
/** Test for equality (for unit tests and debug assertions). */
bool equals(const VariableIndexOrdered& other, double tol=0.0) const;
/** Print the variable index (for unit tests and debugging). */
void print(const std::string& str = "VariableIndex: ") const;
/**
* Output dual hypergraph to Metis file format for use with hmetis
* In the dual graph, variables are hyperedges, factors are nodes.
*/
void outputMetisFormat(std::ostream& os) const;
/// @}
/// @name Advanced Interface
/// @{
/**
* Augment the variable index with new factors. This can be used when
* solving problems incrementally.
*/
template<class FG> void augment(const FG& factors);
/**
* Remove entries corresponding to the specified factors.
* NOTE: We intentionally do not decrement nFactors_ because the factor
* indices need to remain consistent. Removing factors from a factor graph
* does not shift the indices of other factors. Also, we keep nFactors_
* one greater than the highest-numbered factor referenced in a VariableIndex.
*
* @param indices The indices of the factors to remove, which must match \c factors
* @param factors The factors being removed, which must symbolically correspond
* exactly to the factors with the specified \c indices that were added.
*/
template<typename CONTAINER, class FG> void remove(const CONTAINER& indices, const FG& factors);
/// Permute the variables in the VariableIndex according to the given permutation
void permuteInPlace(const Permutation& permutation);
/// Permute the variables in the VariableIndex according to the given partial permutation
void permuteInPlace(const Permutation& selector, const Permutation& permutation);
/** Remove unused empty variables at the end of the ordering (in debug mode
* verifies they are empty).
* @param nToRemove The number of unused variables at the end to remove
*/
void removeUnusedAtEnd(size_t nToRemove);
protected:
Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); }
Factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); }
Factor_const_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); }
Factor_const_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); }
/// Internal constructor to allocate a VariableIndex of the requested size
VariableIndexOrdered(size_t nVars) : index_(nVars), nFactors_(0), nEntries_(0) {}
/// Internal check of the validity of a variable
void checkVar(Index variable) const { assert(variable < index_.size()); }
/// Internal function to populate the variable index from a factor graph
template<class FG> void fill(const FG& factorGraph);
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(index_);
ar & BOOST_SERIALIZATION_NVP(nFactors_);
ar & BOOST_SERIALIZATION_NVP(nEntries_);
}
};
/* ************************************************************************* */
template<class FG>
void VariableIndexOrdered::fill(const FG& factorGraph) {
gttic(VariableIndex_fill);
// Build index mapping from variable id to factor index
for(size_t fi=0; fi<factorGraph.size(); ++fi) {
if(factorGraph[fi]) {
BOOST_FOREACH(const Index key, factorGraph[fi]->keys()) {
if(key < index_.size()) {
index_[key].push_back(fi);
++ nEntries_;
}
}
}
++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent
}
}
/* ************************************************************************* */
template<class FG>
VariableIndexOrdered::VariableIndexOrdered(const FG& factorGraph) :
nFactors_(0), nEntries_(0)
{
gttic(VariableIndex_constructor);
// If the factor graph is empty, return an empty index because inside this
// if block we assume at least one factor.
if(factorGraph.size() > 0) {
gttic(VariableIndex_constructor_find_highest);
// Find highest-numbered variable
Index maxVar = 0;
BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) {
if(factor) {
BOOST_FOREACH(const Index key, factor->keys()) {
if(key > maxVar)
maxVar = key;
}
}
}
gttoc(VariableIndex_constructor_find_highest);
// Allocate array
gttic(VariableIndex_constructor_allocate);
index_.resize(maxVar+1);
gttoc(VariableIndex_constructor_allocate);
fill(factorGraph);
}
}
/* ************************************************************************* */
template<class FG>
VariableIndexOrdered::VariableIndexOrdered(const FG& factorGraph, Index nVariables) :
nFactors_(0), nEntries_(0)
{
gttic(VariableIndex_constructor_allocate);
index_.resize(nVariables);
gttoc(VariableIndex_constructor_allocate);
fill(factorGraph);
}
/* ************************************************************************* */
template<class FG>
void VariableIndexOrdered::augment(const FG& factors) {
gttic(VariableIndex_augment);
// If the factor graph is empty, return an empty index because inside this
// if block we assume at least one factor.
if(factors.size() > 0) {
// Find highest-numbered variable
Index maxVar = 0;
BOOST_FOREACH(const typename FG::sharedFactor& factor, factors) {
if(factor) {
BOOST_FOREACH(const Index key, factor->keys()) {
if(key > maxVar)
maxVar = key;
}
}
}
// Allocate index
index_.resize(std::max(index_.size(), maxVar+1));
// Augment index mapping from variable id to factor index
size_t orignFactors = nFactors_;
for(size_t fi=0; fi<factors.size(); ++fi) {
if(factors[fi]) {
BOOST_FOREACH(const Index key, factors[fi]->keys()) {
index_[key].push_back(orignFactors + fi);
++ nEntries_;
}
}
++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent
}
}
}
/* ************************************************************************* */
template<typename CONTAINER, class FG>
void VariableIndexOrdered::remove(const CONTAINER& indices, const FG& factors) {
gttic(VariableIndex_remove);
// NOTE: We intentionally do not decrement nFactors_ because the factor
// indices need to remain consistent. Removing factors from a factor graph
// does not shift the indices of other factors. Also, we keep nFactors_
// one greater than the highest-numbered factor referenced in a VariableIndex.
for(size_t fi=0; fi<factors.size(); ++fi)
if(factors[fi]) {
for(size_t ji = 0; ji < factors[fi]->keys().size(); ++ji) {
Factors& factorEntries = index_[factors[fi]->keys()[ji]];
Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), indices[fi]);
if(entry == factorEntries.end())
throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
factorEntries.erase(entry);
-- nEntries_;
}
}
}
}

58
gtsam/inference/VariableIndexUnordered.cpp
View File

@ -1,58 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file VariableIndex.cpp
* @author Richard Roberts
* @date March 26, 2013
*/
#include <iostream>
#include <gtsam/inference/VariableIndexUnordered.h>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
bool VariableIndexUnordered::equals(const VariableIndexUnordered& other, double tol) const {
return this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_
&& this->index_ == other.index_;
}
/* ************************************************************************* */
void VariableIndexUnordered::print(const string& str) const {
cout << str;
cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
BOOST_FOREACH(KeyMap::value_type key_factors, index_) {
cout << "var " << key_factors.first << ":";
BOOST_FOREACH(const size_t factor, key_factors.second)
cout << " " << factor;
cout << "\n";
}
cout.flush();
}
/* ************************************************************************* */
void VariableIndexUnordered::outputMetisFormat(ostream& os) const {
os << size() << " " << nFactors() << "\n";
// run over variables, which will be hyper-edges.
BOOST_FOREACH(KeyMap::value_type key_factors, index_) {
// every variable is a hyper-edge covering its factors
BOOST_FOREACH(const size_t factor, key_factors.second)
os << (factor+1) << " "; // base 1
os << "\n";
}
os << flush;
}
}

182
gtsam/inference/VariableIndexUnordered.h
View File

@ -1,182 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file VariableIndex.h
* @author Richard Roberts
* @date March 26, 2013
*/
#pragma once
#include <vector>
#include <deque>
#include <stdexcept>
#include <boost/foreach.hpp>
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/types.h>
#include <gtsam/base/timing.h>
#include <gtsam/inference/Key.h>
namespace gtsam {
/**
* The VariableIndex class computes and stores the block column structure of a
* factor graph. The factor graph stores a collection of factors, each of
* which involves a set of variables. In contrast, the VariableIndex is built
* from a factor graph prior to elimination, and stores the list of factors
* that involve each variable. This information is stored as a deque of
* lists of factor indices.
* \nosubgrouping
*/
class GTSAM_EXPORT VariableIndexUnordered {
public:
typedef boost::shared_ptr<VariableIndexUnordered> shared_ptr;
typedef FastList<size_t> Factors;
typedef Factors::iterator Factor_iterator;
typedef Factors::const_iterator Factor_const_iterator;
protected:
typedef FastMap<Key,Factors> KeyMap;
KeyMap index_;
size_t nFactors_; // Number of factors in the original factor graph.
size_t nEntries_; // Sum of involved variable counts of each factor.
public:
typedef KeyMap::const_iterator const_iterator;
typedef KeyMap::const_iterator iterator;
typedef KeyMap::value_type value_type;
public:
/// @name Standard Constructors
/// @{
/** Default constructor, creates an empty VariableIndex */
VariableIndexUnordered() : nFactors_(0), nEntries_(0) {}
/**
* Create a VariableIndex that computes and stores the block column structure
* of a factor graph.
*/
template<class FG>
VariableIndexUnordered(const FG& factorGraph) : nFactors_(0), nEntries_(0) { augment(factorGraph); }
/// @}
/// @name Standard Interface
/// @{
/**
* The number of variable entries. This is one greater than the variable
* with the highest index.
*/
Index size() const { return index_.size(); }
/** The number of factors in the original factor graph */
size_t nFactors() const { return nFactors_; }
/** The number of nonzero blocks, i.e. the number of variable-factor entries */
size_t nEntries() const { return nEntries_; }
/** Access a list of factors by variable */
const Factors& operator[](Key variable) const {
KeyMap::const_iterator item = index_.find(variable);
if(item == index_.end())
throw std::invalid_argument("Requested non-existent variable from VariableIndex");
else
return item->second;
}
/// @}
/// @name Testable
/// @{
/** Test for equality (for unit tests and debug assertions). */
bool equals(const VariableIndexUnordered& other, double tol=0.0) const;
/** Print the variable index (for unit tests and debugging). */
void print(const std::string& str = "VariableIndex: ") const;
/**
* Output dual hypergraph to Metis file format for use with hmetis
* In the dual graph, variables are hyperedges, factors are nodes.
*/
void outputMetisFormat(std::ostream& os) const;
/// @}
/// @name Advanced Interface
/// @{
/**
* Augment the variable index with new factors. This can be used when
* solving problems incrementally.
*/
template<class FG>
void augment(const FG& factors);
/**
* Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement
* nFactors_ because the factor indices need to remain consistent. Removing factors from a factor
* graph does not shift the indices of other factors. Also, we keep nFactors_ one greater than
* the highest-numbered factor referenced in a VariableIndex.
*
* @param indices The indices of the factors to remove, which must match \c factors
* @param factors The factors being removed, which must symbolically correspond exactly to the
* factors with the specified \c indices that were added.
*/
template<typename ITERATOR, class FG>
void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors);
/** Remove unused empty variables at the end of the ordering (in debug mode
* verifies they are empty).
* @param nToRemove The number of unused variables at the end to remove
*/
template<typename ITERATOR>
void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey);
/** Iterator to the first variable entry */
const_iterator begin() const { return index_.begin(); }
/** Iterator to the first variable entry */
const_iterator end() const { return index_.end(); }
/** Find the iterator for the requested variable entry */
const_iterator find(Key key) const { return index_.find(key); }
protected:
Factor_iterator factorsBegin(Index variable) { return internalAt(variable).begin(); }
Factor_iterator factorsEnd(Index variable) { return internalAt(variable).end(); }
Factor_const_iterator factorsBegin(Index variable) const { return internalAt(variable).begin(); }
Factor_const_iterator factorsEnd(Index variable) const { return internalAt(variable).end(); }
/// Internal version of 'at' that asserts existence
const Factors& internalAt(Key variable) const {
const KeyMap::const_iterator item = index_.find(variable);
assert(item != index_.end());
return item->second; }
/// Internal version of 'at' that asserts existence
Factors& internalAt(Key variable) {
const KeyMap::iterator item = index_.find(variable);
assert(item != index_.end());
return item->second; }
/// @}
};
}
#include <gtsam/inference/VariableIndexUnordered-inl.h>

8
gtsam/inference/inference-inl.h → gtsam/inference/inferenceOrdered-inl.h
View File

@ -21,7 +21,7 @@
#include <algorithm> #include <algorithm>
// Only for Eclipse parser, inference-inl.h (this file) is included at the bottom of inference.h // Only for Eclipse parser, inference-inl.h (this file) is included at the bottom of inference.h
#include <gtsam/inference/inference.h> #include <gtsam/inference/inferenceOrdered.h>
#include <gtsam/base/FastSet.h> #include <gtsam/base/FastSet.h>
@ -32,7 +32,7 @@ namespace inference {
/* ************************************************************************* */ /* ************************************************************************* */
template<typename CONSTRAINED> template<typename CONSTRAINED>
Permutation::shared_ptr PermutationCOLAMD( Permutation::shared_ptr PermutationCOLAMD(
const VariableIndex& variableIndex, const CONSTRAINED& constrainLast, bool forceOrder) { const VariableIndexOrdered& variableIndex, const CONSTRAINED& constrainLast, bool forceOrder) {
gttic(PermutationCOLAMD_constrained); gttic(PermutationCOLAMD_constrained);
size_t n = variableIndex.size(); size_t n = variableIndex.size();
@ -59,7 +59,7 @@ Permutation::shared_ptr PermutationCOLAMD(
/* ************************************************************************* */ /* ************************************************************************* */
template<typename CONSTRAINED_MAP> template<typename CONSTRAINED_MAP>
Permutation::shared_ptr PermutationCOLAMDGrouped( Permutation::shared_ptr PermutationCOLAMDGrouped(
const VariableIndex& variableIndex, const CONSTRAINED_MAP& constraints) { const VariableIndexOrdered& variableIndex, const CONSTRAINED_MAP& constraints) {
gttic(PermutationCOLAMD_grouped); gttic(PermutationCOLAMD_grouped);
size_t n = variableIndex.size(); size_t n = variableIndex.size();
std::vector<int> cmember(n, 0); std::vector<int> cmember(n, 0);
@ -75,7 +75,7 @@ Permutation::shared_ptr PermutationCOLAMDGrouped(
} }
/* ************************************************************************* */ /* ************************************************************************* */
inline Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex) { inline Permutation::shared_ptr PermutationCOLAMD(const VariableIndexOrdered& variableIndex) {
gttic(PermutationCOLAMD_unconstrained); gttic(PermutationCOLAMD_unconstrained);
size_t n = variableIndex.size(); size_t n = variableIndex.size();
std::vector<int> cmember(n, 0); std::vector<int> cmember(n, 0);

8
gtsam/inference/inference.cpp → gtsam/inference/inferenceOrdered.cpp
View File

@ -16,8 +16,8 @@
* @author Richard Roberts * @author Richard Roberts
*/ */
#include <gtsam/inference/inference.h> #include <gtsam/inference/inferenceOrdered.h>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <stdexcept> #include <stdexcept>
@ -32,7 +32,7 @@ namespace gtsam {
namespace inference { namespace inference {
/* ************************************************************************* */ /* ************************************************************************* */
Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember) { Permutation::shared_ptr PermutationCOLAMD_(const VariableIndexOrdered& variableIndex, std::vector<int>& cmember) {
gttic(PermutationCOLAMD_internal); gttic(PermutationCOLAMD_internal);
gttic(Prepare); gttic(Prepare);
@ -47,7 +47,7 @@ Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, s
p[0] = 0; p[0] = 0;
int count = 0; int count = 0;
for(Index var = 0; var < variableIndex.size(); ++var) { for(Index var = 0; var < variableIndex.size(); ++var) {
const VariableIndex::Factors& column(variableIndex[var]); const VariableIndexOrdered::Factors& column(variableIndex[var]);
size_t lastFactorId = numeric_limits<size_t>::max(); size_t lastFactorId = numeric_limits<size_t>::max();
BOOST_FOREACH(size_t factorIndex, column) { BOOST_FOREACH(size_t factorIndex, column) {
if(lastFactorId != numeric_limits<size_t>::max()) if(lastFactorId != numeric_limits<size_t>::max())

14
gtsam/inference/inference.h → gtsam/inference/inferenceOrdered.h
View File

@ -19,8 +19,8 @@
#pragma once #pragma once
#include <gtsam/inference/VariableIndex.h> #include <gtsam/inference/VariableIndexOrdered.h>
#include <gtsam/inference/Permutation.h> #include <gtsam/inference/PermutationOrdered.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/optional.hpp> #include <boost/optional.hpp>
@ -35,7 +35,7 @@ namespace gtsam {
* Compute a permutation (variable ordering) using colamd * Compute a permutation (variable ordering) using colamd
*/ */
GTSAM_EXPORT Permutation::shared_ptr PermutationCOLAMD( GTSAM_EXPORT Permutation::shared_ptr PermutationCOLAMD(
const VariableIndex& variableIndex); const VariableIndexOrdered& variableIndex);
/** /**
* Compute a permutation (variable ordering) using constrained colamd to move * Compute a permutation (variable ordering) using constrained colamd to move
@ -47,7 +47,7 @@ namespace gtsam {
*/ */
template<typename CONSTRAINED> template<typename CONSTRAINED>
Permutation::shared_ptr PermutationCOLAMD( Permutation::shared_ptr PermutationCOLAMD(
const VariableIndex& variableIndex, const CONSTRAINED& constrainLast, bool forceOrder=false); const VariableIndexOrdered& variableIndex, const CONSTRAINED& constrainLast, bool forceOrder=false);
/** /**
* Compute a permutation of variable ordering using constrained colamd to * Compute a permutation of variable ordering using constrained colamd to
@ -59,7 +59,7 @@ namespace gtsam {
*/ */
template<typename CONSTRAINED_MAP> template<typename CONSTRAINED_MAP>
Permutation::shared_ptr PermutationCOLAMDGrouped( Permutation::shared_ptr PermutationCOLAMDGrouped(
const VariableIndex& variableIndex, const CONSTRAINED_MAP& constraints); const VariableIndexOrdered& variableIndex, const CONSTRAINED_MAP& constraints);
/** /**
* Compute a CCOLAMD permutation using the constraint groups in cmember. * Compute a CCOLAMD permutation using the constraint groups in cmember.
@ -73,10 +73,10 @@ namespace gtsam {
* AGC: does cmember change? * AGC: does cmember change?
*/ */
GTSAM_EXPORT Permutation::shared_ptr PermutationCOLAMD_( GTSAM_EXPORT Permutation::shared_ptr PermutationCOLAMD_(
const VariableIndex& variableIndex, std::vector<int>& cmember); const VariableIndexOrdered& variableIndex, std::vector<int>& cmember);
} // \namespace inference } // \namespace inference
} // \namespace gtsam } // \namespace gtsam
#include <gtsam/inference/inference-inl.h> #include <gtsam/inference/inferenceOrdered-inl.h>

364
gtsam/inference/tests/testBayesTreeObsolete.cpp
View File

@ -26,10 +26,10 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/BayesTree.h> #include <gtsam/inference/BayesTreeOrdered.h>
#include <gtsam/inference/IndexFactor.h> #include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/SymbolicSequentialSolver.h> #include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -38,18 +38,18 @@ using namespace gtsam;
//// SLAM example from RSS sqrtSAM paper //// SLAM example from RSS sqrtSAM paper
static const Index _x3_=0, _x2_=1; static const Index _x3_=0, _x2_=1;
//static const Index _x1_=2, _l2_=3, _l1_=4; // unused //static const Index _x1_=2, _l2_=3, _l1_=4; // unused
//IndexConditional::shared_ptr //IndexConditionalOrdered::shared_ptr
// x3(new IndexConditional(_x3_)), // x3(new IndexConditionalOrdered(_x3_)),
// x2(new IndexConditional(_x2_,_x3_)), // x2(new IndexConditionalOrdered(_x2_,_x3_)),
// x1(new IndexConditional(_x1_,_x2_,_x3_)), // x1(new IndexConditionalOrdered(_x1_,_x2_,_x3_)),
// l1(new IndexConditional(_l1_,_x1_,_x2_)), // l1(new IndexConditionalOrdered(_l1_,_x1_,_x2_)),
// l2(new IndexConditional(_l2_,_x1_,_x3_)); // l2(new IndexConditionalOrdered(_l2_,_x1_,_x3_));
// //
//// Bayes Tree for sqrtSAM example //// Bayes Tree for sqrtSAM example
//SymbolicBayesTree createSlamSymbolicBayesTree(){ //SymbolicBayesTreeOrdered createSlamSymbolicBayesTree(){
// // Create using insert // // Create using insert
//// Ordering slamOrdering; slamOrdering += _x3_, _x2_, _x1_, _l2_, _l1_; //// Ordering slamOrdering; slamOrdering += _x3_, _x2_, _x1_, _l2_, _l1_;
// SymbolicBayesTree bayesTree_slam; // SymbolicBayesTreeOrdered bayesTree_slam;
// bayesTree_slam.insert(x3); // bayesTree_slam.insert(x3);
// bayesTree_slam.insert(x2); // bayesTree_slam.insert(x2);
// bayesTree_slam.insert(x1); // bayesTree_slam.insert(x1);
@ -61,36 +61,36 @@ static const Index _x3_=0, _x2_=1;
/* ************************************************************************* */ /* ************************************************************************* */
// Conditionals for ASIA example from the tutorial with A and D evidence // Conditionals for ASIA example from the tutorial with A and D evidence
static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5; static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5;
static IndexConditional::shared_ptr static IndexConditionalOrdered::shared_ptr
B(new IndexConditional(_B_)), B(new IndexConditionalOrdered(_B_)),
L(new IndexConditional(_L_, _B_)), L(new IndexConditionalOrdered(_L_, _B_)),
E(new IndexConditional(_E_, _L_, _B_)), E(new IndexConditionalOrdered(_E_, _L_, _B_)),
S(new IndexConditional(_S_, _L_, _B_)), S(new IndexConditionalOrdered(_S_, _L_, _B_)),
T(new IndexConditional(_T_, _E_, _L_)), T(new IndexConditionalOrdered(_T_, _E_, _L_)),
X(new IndexConditional(_X_, _E_)); X(new IndexConditionalOrdered(_X_, _E_));
// Cliques // Cliques
static IndexConditional::shared_ptr static IndexConditionalOrdered::shared_ptr
ELB(IndexConditional::FromKeys(cref_list_of<3>(_E_)(_L_)(_B_), 3)); ELB(IndexConditionalOrdered::FromKeys(cref_list_of<3>(_E_)(_L_)(_B_), 3));
// Bayes Tree for Asia example // Bayes Tree for Asia example
static SymbolicBayesTree createAsiaSymbolicBayesTree() { static SymbolicBayesTreeOrdered createAsiaSymbolicBayesTree() {
SymbolicBayesTree bayesTree; SymbolicBayesTreeOrdered bayesTree;
// Ordering asiaOrdering; asiaOrdering += _X_, _T_, _S_, _E_, _L_, _B_; // Ordering asiaOrdering; asiaOrdering += _X_, _T_, _S_, _E_, _L_, _B_;
SymbolicBayesTree::insert(bayesTree, B); SymbolicBayesTreeOrdered::insert(bayesTree, B);
SymbolicBayesTree::insert(bayesTree, L); SymbolicBayesTreeOrdered::insert(bayesTree, L);
SymbolicBayesTree::insert(bayesTree, E); SymbolicBayesTreeOrdered::insert(bayesTree, E);
SymbolicBayesTree::insert(bayesTree, S); SymbolicBayesTreeOrdered::insert(bayesTree, S);
SymbolicBayesTree::insert(bayesTree, T); SymbolicBayesTreeOrdered::insert(bayesTree, T);
SymbolicBayesTree::insert(bayesTree, X); SymbolicBayesTreeOrdered::insert(bayesTree, X);
return bayesTree; return bayesTree;
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesTree, constructor ) TEST( BayesTreeOrdered, constructor )
{ {
// Create using insert // Create using insert
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree();
bayesTree.print("bayesTree (ordered): "); bayesTree.print("bayesTree (ordered): ");
@ -99,18 +99,18 @@ TEST( BayesTree, constructor )
EXPECT(!bayesTree.empty()); EXPECT(!bayesTree.empty());
// Check root // Check root
boost::shared_ptr<IndexConditional> actual_root = bayesTree.root()->conditional(); boost::shared_ptr<IndexConditionalOrdered> actual_root = bayesTree.root()->conditional();
CHECK(assert_equal(*ELB,*actual_root)); CHECK(assert_equal(*ELB,*actual_root));
// Create from symbolic Bayes chain in which we want to discover cliques // Create from symbolic Bayes chain in which we want to discover cliques
BayesNet<IndexConditional> ASIA; BayesNetOrdered<IndexConditionalOrdered> ASIA;
ASIA.push_back(X); ASIA.push_back(X);
ASIA.push_back(T); ASIA.push_back(T);
ASIA.push_back(S); ASIA.push_back(S);
ASIA.push_back(E); ASIA.push_back(E);
ASIA.push_back(L); ASIA.push_back(L);
ASIA.push_back(B); ASIA.push_back(B);
SymbolicBayesTree bayesTree2(ASIA); SymbolicBayesTreeOrdered bayesTree2(ASIA);
// Check whether the same // Check whether the same
CHECK(assert_equal(bayesTree,bayesTree2)); CHECK(assert_equal(bayesTree,bayesTree2));
@ -130,14 +130,14 @@ TEST( BayesTree, constructor )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(BayesTree, clear) TEST(BayesTreeOrdered, clear)
{ {
// SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); // SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree();
// bayesTree.clear(); // bayesTree.clear();
// //
// SymbolicBayesTree expected; // SymbolicBayesTreeOrdered expected;
// //
// // Check whether cleared BayesTree is equal to a new BayesTree // // Check whether cleared BayesTreeOrdered is equal to a new BayesTreeOrdered
// CHECK(assert_equal(expected, bayesTree)); // CHECK(assert_equal(expected, bayesTree));
} }
@ -148,144 +148,144 @@ Bayes Tree for testing conversion to a forest of orphans needed for incremental.
D|C F|E D|C F|E
*/ */
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesTree, removePath ) TEST( BayesTreeOrdered, removePath )
{ {
const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0; const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0;
IndexConditional::shared_ptr IndexConditionalOrdered::shared_ptr
A(new IndexConditional(_A_)), A(new IndexConditionalOrdered(_A_)),
B(new IndexConditional(_B_, _A_)), B(new IndexConditionalOrdered(_B_, _A_)),
C(new IndexConditional(_C_, _A_)), C(new IndexConditionalOrdered(_C_, _A_)),
D(new IndexConditional(_D_, _C_)), D(new IndexConditionalOrdered(_D_, _C_)),
E(new IndexConditional(_E_, _B_)), E(new IndexConditionalOrdered(_E_, _B_)),
F(new IndexConditional(_F_, _E_)); F(new IndexConditionalOrdered(_F_, _E_));
SymbolicBayesTree bayesTree; SymbolicBayesTreeOrdered bayesTree;
EXPECT(bayesTree.empty()); EXPECT(bayesTree.empty());
// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_; // Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_;
SymbolicBayesTree::insert(bayesTree, A); SymbolicBayesTreeOrdered::insert(bayesTree, A);
SymbolicBayesTree::insert(bayesTree, B); SymbolicBayesTreeOrdered::insert(bayesTree, B);
SymbolicBayesTree::insert(bayesTree, C); SymbolicBayesTreeOrdered::insert(bayesTree, C);
SymbolicBayesTree::insert(bayesTree, D); SymbolicBayesTreeOrdered::insert(bayesTree, D);
SymbolicBayesTree::insert(bayesTree, E); SymbolicBayesTreeOrdered::insert(bayesTree, E);
SymbolicBayesTree::insert(bayesTree, F); SymbolicBayesTreeOrdered::insert(bayesTree, F);
// remove C, expected outcome: factor graph with ABC, // remove C, expected outcome: factor graph with ABC,
// Bayes Tree now contains two orphan trees: D|C and E|B,F|E // Bayes Tree now contains two orphan trees: D|C and E|B,F|E
SymbolicFactorGraph expected; SymbolicFactorGraphOrdered expected;
expected.push_factor(_B_,_A_); expected.push_factor(_B_,_A_);
// expected.push_factor(_A_); // expected.push_factor(_A_);
expected.push_factor(_C_,_A_); expected.push_factor(_C_,_A_);
SymbolicBayesTree::Cliques expectedOrphans; SymbolicBayesTreeOrdered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_D_], bayesTree[_E_]; expectedOrphans += bayesTree[_D_], bayesTree[_E_];
BayesNet<IndexConditional> bn; BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTreeOrdered::Cliques orphans;
bayesTree.removePath(bayesTree[_C_], bn, orphans); bayesTree.removePath(bayesTree[_C_], bn, orphans);
SymbolicFactorGraph factors(bn); SymbolicFactorGraphOrdered factors(bn);
CHECK(assert_equal((SymbolicFactorGraph)expected, factors)); CHECK(assert_equal((SymbolicFactorGraphOrdered)expected, factors));
CHECK(assert_equal(expectedOrphans, orphans)); CHECK(assert_equal(expectedOrphans, orphans));
// remove E: factor graph with EB; E|B removed from second orphan tree // remove E: factor graph with EB; E|B removed from second orphan tree
SymbolicFactorGraph expected2; SymbolicFactorGraphOrdered expected2;
expected2.push_factor(_E_,_B_); expected2.push_factor(_E_,_B_);
SymbolicBayesTree::Cliques expectedOrphans2; SymbolicBayesTreeOrdered::Cliques expectedOrphans2;
expectedOrphans2 += bayesTree[_F_]; expectedOrphans2 += bayesTree[_F_];
BayesNet<IndexConditional> bn2; BayesNetOrdered<IndexConditionalOrdered> bn2;
SymbolicBayesTree::Cliques orphans2; SymbolicBayesTreeOrdered::Cliques orphans2;
bayesTree.removePath(bayesTree[_E_], bn2, orphans2); bayesTree.removePath(bayesTree[_E_], bn2, orphans2);
SymbolicFactorGraph factors2(bn2); SymbolicFactorGraphOrdered factors2(bn2);
CHECK(assert_equal((SymbolicFactorGraph)expected2, factors2)); CHECK(assert_equal((SymbolicFactorGraphOrdered)expected2, factors2));
CHECK(assert_equal(expectedOrphans2, orphans2)); CHECK(assert_equal(expectedOrphans2, orphans2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesTree, removePath2 ) TEST( BayesTreeOrdered, removePath2 )
{ {
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree();
// Call remove-path with clique B // Call remove-path with clique B
BayesNet<IndexConditional> bn; BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTreeOrdered::Cliques orphans;
bayesTree.removePath(bayesTree[_B_], bn, orphans); bayesTree.removePath(bayesTree[_B_], bn, orphans);
SymbolicFactorGraph factors(bn); SymbolicFactorGraphOrdered factors(bn);
// Check expected outcome // Check expected outcome
SymbolicFactorGraph expected; SymbolicFactorGraphOrdered expected;
expected.push_factor(_E_,_L_,_B_); expected.push_factor(_E_,_L_,_B_);
// expected.push_factor(_L_,_B_); // expected.push_factor(_L_,_B_);
// expected.push_factor(_B_); // expected.push_factor(_B_);
CHECK(assert_equal(expected, factors)); CHECK(assert_equal(expected, factors));
SymbolicBayesTree::Cliques expectedOrphans; SymbolicBayesTreeOrdered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_S_], bayesTree[_T_], bayesTree[_X_]; expectedOrphans += bayesTree[_S_], bayesTree[_T_], bayesTree[_X_];
CHECK(assert_equal(expectedOrphans, orphans)); CHECK(assert_equal(expectedOrphans, orphans));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesTree, removePath3 ) TEST( BayesTreeOrdered, removePath3 )
{ {
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree();
// Call remove-path with clique S // Call remove-path with clique S
BayesNet<IndexConditional> bn; BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTreeOrdered::Cliques orphans;
bayesTree.removePath(bayesTree[_S_], bn, orphans); bayesTree.removePath(bayesTree[_S_], bn, orphans);
SymbolicFactorGraph factors(bn); SymbolicFactorGraphOrdered factors(bn);
// Check expected outcome // Check expected outcome
SymbolicFactorGraph expected; SymbolicFactorGraphOrdered expected;
expected.push_factor(_E_,_L_,_B_); expected.push_factor(_E_,_L_,_B_);
// expected.push_factor(_L_,_B_); // expected.push_factor(_L_,_B_);
// expected.push_factor(_B_); // expected.push_factor(_B_);
expected.push_factor(_S_,_L_,_B_); expected.push_factor(_S_,_L_,_B_);
CHECK(assert_equal(expected, factors)); CHECK(assert_equal(expected, factors));
SymbolicBayesTree::Cliques expectedOrphans; SymbolicBayesTreeOrdered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_T_], bayesTree[_X_]; expectedOrphans += bayesTree[_T_], bayesTree[_X_];
CHECK(assert_equal(expectedOrphans, orphans)); CHECK(assert_equal(expectedOrphans, orphans));
} }
void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, SymbolicBayesTree::Cliques& cliques) { void getAllCliques(const SymbolicBayesTreeOrdered::sharedClique& subtree, SymbolicBayesTreeOrdered::Cliques& cliques) {
// Check if subtree exists // Check if subtree exists
if (subtree) { if (subtree) {
cliques.push_back(subtree); cliques.push_back(subtree);
// Recursive call over all child cliques // Recursive call over all child cliques
BOOST_FOREACH(SymbolicBayesTree::sharedClique& childClique, subtree->children()) { BOOST_FOREACH(SymbolicBayesTreeOrdered::sharedClique& childClique, subtree->children()) {
getAllCliques(childClique,cliques); getAllCliques(childClique,cliques);
} }
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesTree, shortcutCheck ) TEST( BayesTreeOrdered, shortcutCheck )
{ {
const Index _A_=6, _B_=5, _C_=4, _D_=3, _E_=2, _F_=1, _G_=0; const Index _A_=6, _B_=5, _C_=4, _D_=3, _E_=2, _F_=1, _G_=0;
IndexConditional::shared_ptr IndexConditionalOrdered::shared_ptr
A(new IndexConditional(_A_)), A(new IndexConditionalOrdered(_A_)),
B(new IndexConditional(_B_, _A_)), B(new IndexConditionalOrdered(_B_, _A_)),
C(new IndexConditional(_C_, _A_)), C(new IndexConditionalOrdered(_C_, _A_)),
D(new IndexConditional(_D_, _C_)), D(new IndexConditionalOrdered(_D_, _C_)),
E(new IndexConditional(_E_, _B_)), E(new IndexConditionalOrdered(_E_, _B_)),
F(new IndexConditional(_F_, _E_)), F(new IndexConditionalOrdered(_F_, _E_)),
G(new IndexConditional(_G_, _F_)); G(new IndexConditionalOrdered(_G_, _F_));
SymbolicBayesTree bayesTree; SymbolicBayesTreeOrdered bayesTree;
// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_; // Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_;
SymbolicBayesTree::insert(bayesTree, A); SymbolicBayesTreeOrdered::insert(bayesTree, A);
SymbolicBayesTree::insert(bayesTree, B); SymbolicBayesTreeOrdered::insert(bayesTree, B);
SymbolicBayesTree::insert(bayesTree, C); SymbolicBayesTreeOrdered::insert(bayesTree, C);
SymbolicBayesTree::insert(bayesTree, D); SymbolicBayesTreeOrdered::insert(bayesTree, D);
SymbolicBayesTree::insert(bayesTree, E); SymbolicBayesTreeOrdered::insert(bayesTree, E);
SymbolicBayesTree::insert(bayesTree, F); SymbolicBayesTreeOrdered::insert(bayesTree, F);
SymbolicBayesTree::insert(bayesTree, G); SymbolicBayesTreeOrdered::insert(bayesTree, G);
//bayesTree.print("BayesTree"); //bayesTree.print("BayesTreeOrdered");
//bayesTree.saveGraph("BT1.dot"); //bayesTree.saveGraph("BT1.dot");
SymbolicBayesTree::sharedClique rootClique= bayesTree.root(); SymbolicBayesTreeOrdered::sharedClique rootClique= bayesTree.root();
//rootClique->printTree(); //rootClique->printTree();
SymbolicBayesTree::Cliques allCliques; SymbolicBayesTreeOrdered::Cliques allCliques;
getAllCliques(rootClique,allCliques); getAllCliques(rootClique,allCliques);
BayesNet<IndexConditional> bn; BayesNetOrdered<IndexConditionalOrdered> bn;
BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { BOOST_FOREACH(SymbolicBayesTreeOrdered::sharedClique& clique, allCliques) {
//clique->print("Clique#"); //clique->print("Clique#");
bn = clique->shortcut(rootClique, &EliminateSymbolic); bn = clique->shortcut(rootClique, &EliminateSymbolic);
//bn.print("Shortcut:\n"); //bn.print("Shortcut:\n");
@ -294,13 +294,13 @@ TEST( BayesTree, shortcutCheck )
// Check if all the cached shortcuts are cleared // Check if all the cached shortcuts are cleared
rootClique->deleteCachedShortcuts(); rootClique->deleteCachedShortcuts();
BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { BOOST_FOREACH(SymbolicBayesTreeOrdered::sharedClique& clique, allCliques) {
bool notCleared = clique->cachedSeparatorMarginal(); bool notCleared = clique->cachedSeparatorMarginal();
CHECK( notCleared == false); CHECK( notCleared == false);
} }
EXPECT_LONGS_EQUAL(0, rootClique->numCachedSeparatorMarginals()); EXPECT_LONGS_EQUAL(0, rootClique->numCachedSeparatorMarginals());
// BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { // BOOST_FOREACH(SymbolicBayesTreeOrdered::sharedClique& clique, allCliques) {
// clique->print("Clique#"); // clique->print("Clique#");
// if(clique->cachedShortcut()){ // if(clique->cachedShortcut()){
// bn = clique->cachedShortcut().get(); // bn = clique->cachedShortcut().get();
@ -313,105 +313,105 @@ TEST( BayesTree, shortcutCheck )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesTree, removeTop ) TEST( BayesTreeOrdered, removeTop )
{ {
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree();
// create a new factor to be inserted // create a new factor to be inserted
boost::shared_ptr<IndexFactor> newFactor(new IndexFactor(_S_,_B_)); boost::shared_ptr<IndexFactorOrdered> newFactor(new IndexFactorOrdered(_S_,_B_));
// Remove the contaminated part of the Bayes tree // Remove the contaminated part of the Bayes tree
BayesNet<IndexConditional> bn; BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTreeOrdered::Cliques orphans;
list<Index> keys; keys += _B_,_S_; list<Index> keys; keys += _B_,_S_;
bayesTree.removeTop(keys, bn, orphans); bayesTree.removeTop(keys, bn, orphans);
SymbolicFactorGraph factors(bn); SymbolicFactorGraphOrdered factors(bn);
// Check expected outcome // Check expected outcome
SymbolicFactorGraph expected; SymbolicFactorGraphOrdered expected;
expected.push_factor(_E_,_L_,_B_); expected.push_factor(_E_,_L_,_B_);
// expected.push_factor(_L_,_B_); // expected.push_factor(_L_,_B_);
// expected.push_factor(_B_); // expected.push_factor(_B_);
expected.push_factor(_S_,_L_,_B_); expected.push_factor(_S_,_L_,_B_);
CHECK(assert_equal(expected, factors)); CHECK(assert_equal(expected, factors));
SymbolicBayesTree::Cliques expectedOrphans; SymbolicBayesTreeOrdered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_T_], bayesTree[_X_]; expectedOrphans += bayesTree[_T_], bayesTree[_X_];
CHECK(assert_equal(expectedOrphans, orphans)); CHECK(assert_equal(expectedOrphans, orphans));
// Try removeTop again with a factor that should not change a thing // Try removeTop again with a factor that should not change a thing
boost::shared_ptr<IndexFactor> newFactor2(new IndexFactor(_B_)); boost::shared_ptr<IndexFactorOrdered> newFactor2(new IndexFactorOrdered(_B_));
BayesNet<IndexConditional> bn2; BayesNetOrdered<IndexConditionalOrdered> bn2;
SymbolicBayesTree::Cliques orphans2; SymbolicBayesTreeOrdered::Cliques orphans2;
keys.clear(); keys += _B_; keys.clear(); keys += _B_;
bayesTree.removeTop(keys, bn2, orphans2); bayesTree.removeTop(keys, bn2, orphans2);
SymbolicFactorGraph factors2(bn2); SymbolicFactorGraphOrdered factors2(bn2);
SymbolicFactorGraph expected2; SymbolicFactorGraphOrdered expected2;
CHECK(assert_equal(expected2, factors2)); CHECK(assert_equal(expected2, factors2));
SymbolicBayesTree::Cliques expectedOrphans2; SymbolicBayesTreeOrdered::Cliques expectedOrphans2;
CHECK(assert_equal(expectedOrphans2, orphans2)); CHECK(assert_equal(expectedOrphans2, orphans2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesTree, removeTop2 ) TEST( BayesTreeOrdered, removeTop2 )
{ {
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree();
// create two factors to be inserted // create two factors to be inserted
SymbolicFactorGraph newFactors; SymbolicFactorGraphOrdered newFactors;
newFactors.push_factor(_B_); newFactors.push_factor(_B_);
newFactors.push_factor(_S_); newFactors.push_factor(_S_);
// Remove the contaminated part of the Bayes tree // Remove the contaminated part of the Bayes tree
BayesNet<IndexConditional> bn; BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTreeOrdered::Cliques orphans;
list<Index> keys; keys += _B_,_S_; list<Index> keys; keys += _B_,_S_;
bayesTree.removeTop(keys, bn, orphans); bayesTree.removeTop(keys, bn, orphans);
SymbolicFactorGraph factors(bn); SymbolicFactorGraphOrdered factors(bn);
// Check expected outcome // Check expected outcome
SymbolicFactorGraph expected; SymbolicFactorGraphOrdered expected;
expected.push_factor(_E_,_L_,_B_); expected.push_factor(_E_,_L_,_B_);
// expected.push_factor(_L_,_B_); // expected.push_factor(_L_,_B_);
// expected.push_factor(_B_); // expected.push_factor(_B_);
expected.push_factor(_S_,_L_,_B_); expected.push_factor(_S_,_L_,_B_);
CHECK(assert_equal(expected, factors)); CHECK(assert_equal(expected, factors));
SymbolicBayesTree::Cliques expectedOrphans; SymbolicBayesTreeOrdered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_T_], bayesTree[_X_]; expectedOrphans += bayesTree[_T_], bayesTree[_X_];
CHECK(assert_equal(expectedOrphans, orphans)); CHECK(assert_equal(expectedOrphans, orphans));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesTree, removeTop3 ) TEST( BayesTreeOrdered, removeTop3 )
{ {
const Index _x4_=5, _l5_=6; const Index _x4_=5, _l5_=6;
// simple test case that failed after COLAMD was fixed/activated // simple test case that failed after COLAMD was fixed/activated
IndexConditional::shared_ptr IndexConditionalOrdered::shared_ptr
X(new IndexConditional(_l5_)), X(new IndexConditionalOrdered(_l5_)),
A(new IndexConditional(_x4_, _l5_)), A(new IndexConditionalOrdered(_x4_, _l5_)),
B(new IndexConditional(_x2_, _x4_)), B(new IndexConditionalOrdered(_x2_, _x4_)),
C(new IndexConditional(_x3_, _x2_)); C(new IndexConditionalOrdered(_x3_, _x2_));
// Ordering newOrdering; // Ordering newOrdering;
// newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_; // newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_;
SymbolicBayesTree bayesTree; SymbolicBayesTreeOrdered bayesTree;
SymbolicBayesTree::insert(bayesTree, X); SymbolicBayesTreeOrdered::insert(bayesTree, X);
SymbolicBayesTree::insert(bayesTree, A); SymbolicBayesTreeOrdered::insert(bayesTree, A);
SymbolicBayesTree::insert(bayesTree, B); SymbolicBayesTreeOrdered::insert(bayesTree, B);
SymbolicBayesTree::insert(bayesTree, C); SymbolicBayesTreeOrdered::insert(bayesTree, C);
// remove all // remove all
list<Index> keys; list<Index> keys;
keys += _l5_, _x2_, _x3_, _x4_; keys += _l5_, _x2_, _x3_, _x4_;
BayesNet<IndexConditional> bn; BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTreeOrdered::Cliques orphans;
bayesTree.removeTop(keys, bn, orphans); bayesTree.removeTop(keys, bn, orphans);
SymbolicFactorGraph factors(bn); SymbolicFactorGraphOrdered factors(bn);
CHECK(orphans.size() == 0); CHECK(orphans.size() == 0);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesTree, permute ) TEST( BayesTreeOrdered, permute )
{ {
// creates a permutation and ensures that the nodes listing is updated // creates a permutation and ensures that the nodes listing is updated
@ -437,20 +437,20 @@ TEST( BayesTree, permute )
gtsam::internal::Reduction inv_reduction = gtsam::internal::Reduction::CreateAsInverse(expReducingPermutation); gtsam::internal::Reduction inv_reduction = gtsam::internal::Reduction::CreateAsInverse(expReducingPermutation);
// Build a bayes tree around reduced keys as if just eliminated from subset of factors/variables // Build a bayes tree around reduced keys as if just eliminated from subset of factors/variables
IndexConditional::shared_ptr IndexConditionalOrdered::shared_ptr
A(new IndexConditional(_A_)), A(new IndexConditionalOrdered(_A_)),
B(new IndexConditional(_B_, _A_)), B(new IndexConditionalOrdered(_B_, _A_)),
C(new IndexConditional(_C_, _A_)), C(new IndexConditionalOrdered(_C_, _A_)),
D(new IndexConditional(_D_, _C_)), D(new IndexConditionalOrdered(_D_, _C_)),
E(new IndexConditional(_E_, _B_)), E(new IndexConditionalOrdered(_E_, _B_)),
F(new IndexConditional(_F_, _E_)); F(new IndexConditionalOrdered(_F_, _E_));
SymbolicBayesTree bayesTreeReduced; SymbolicBayesTreeOrdered bayesTreeReduced;
SymbolicBayesTree::insert(bayesTreeReduced, A); SymbolicBayesTreeOrdered::insert(bayesTreeReduced, A);
SymbolicBayesTree::insert(bayesTreeReduced, B); SymbolicBayesTreeOrdered::insert(bayesTreeReduced, B);
SymbolicBayesTree::insert(bayesTreeReduced, C); SymbolicBayesTreeOrdered::insert(bayesTreeReduced, C);
SymbolicBayesTree::insert(bayesTreeReduced, D); SymbolicBayesTreeOrdered::insert(bayesTreeReduced, D);
SymbolicBayesTree::insert(bayesTreeReduced, E); SymbolicBayesTreeOrdered::insert(bayesTreeReduced, E);
SymbolicBayesTree::insert(bayesTreeReduced, F); SymbolicBayesTreeOrdered::insert(bayesTreeReduced, F);
// bayesTreeReduced.print("Reduced bayes tree"); // bayesTreeReduced.print("Reduced bayes tree");
// P( 4 5) // P( 4 5)
@ -460,7 +460,7 @@ TEST( BayesTree, permute )
// P( 0 | 1) // P( 0 | 1)
// Apply the permutation - should add placeholders for variables not present in nodes // Apply the permutation - should add placeholders for variables not present in nodes
SymbolicBayesTree actBayesTree = *bayesTreeReduced.clone(); SymbolicBayesTreeOrdered actBayesTree = *bayesTreeReduced.clone();
actBayesTree.permuteWithInverse(expReducingPermutation); actBayesTree.permuteWithInverse(expReducingPermutation);
// actBayesTree.print("Full bayes tree"); // actBayesTree.print("Full bayes tree");
@ -472,14 +472,14 @@ TEST( BayesTree, permute )
// check keys in cliques // check keys in cliques
std::vector<Index> expRootIndices; expRootIndices += _B0_, _A0_; std::vector<Index> expRootIndices; expRootIndices += _B0_, _A0_;
IndexConditional::shared_ptr IndexConditionalOrdered::shared_ptr
expRoot(new IndexConditional(expRootIndices, 2)), // root expRoot(new IndexConditionalOrdered(expRootIndices, 2)), // root
A0(new IndexConditional(_A0_)), A0(new IndexConditionalOrdered(_A0_)),
B0(new IndexConditional(_B0_, _A0_)), B0(new IndexConditionalOrdered(_B0_, _A0_)),
C0(new IndexConditional(_C0_, _A0_)), // leaf level 1 C0(new IndexConditionalOrdered(_C0_, _A0_)), // leaf level 1
D0(new IndexConditional(_D0_, _C0_)), // leaf level 2 D0(new IndexConditionalOrdered(_D0_, _C0_)), // leaf level 2
E0(new IndexConditional(_E0_, _B0_)), // leaf level 2 E0(new IndexConditionalOrdered(_E0_, _B0_)), // leaf level 2
F0(new IndexConditional(_F0_, _E0_)); // leaf level 3 F0(new IndexConditionalOrdered(_F0_, _E0_)); // leaf level 3
CHECK(actBayesTree.root()); CHECK(actBayesTree.root());
EXPECT(assert_equal(*expRoot, *actBayesTree.root()->conditional())); EXPECT(assert_equal(*expRoot, *actBayesTree.root()->conditional()));
@ -491,13 +491,13 @@ TEST( BayesTree, permute )
// check nodes structure // check nodes structure
LONGS_EQUAL(9, actBayesTree.nodes().size()); LONGS_EQUAL(9, actBayesTree.nodes().size());
SymbolicBayesTree expFullTree; SymbolicBayesTreeOrdered expFullTree;
SymbolicBayesTree::insert(expFullTree, A0); SymbolicBayesTreeOrdered::insert(expFullTree, A0);
SymbolicBayesTree::insert(expFullTree, B0); SymbolicBayesTreeOrdered::insert(expFullTree, B0);
SymbolicBayesTree::insert(expFullTree, C0); SymbolicBayesTreeOrdered::insert(expFullTree, C0);
SymbolicBayesTree::insert(expFullTree, D0); SymbolicBayesTreeOrdered::insert(expFullTree, D0);
SymbolicBayesTree::insert(expFullTree, E0); SymbolicBayesTreeOrdered::insert(expFullTree, E0);
SymbolicBayesTree::insert(expFullTree, F0); SymbolicBayesTreeOrdered::insert(expFullTree, F0);
EXPECT(assert_equal(expFullTree, actBayesTree)); EXPECT(assert_equal(expFullTree, actBayesTree));
} }
@ -508,11 +508,11 @@ TEST( BayesTree, permute )
// * | / \ | // * | / \ |
// * x1 / \ x6 // * x1 / \ x6
// */ // */
//TEST( BayesTree, insert ) //TEST( BayesTreeOrdered, insert )
//{ //{
// // construct bayes tree by split the graph along the separator x3 - x4 // // construct bayes tree by split the graph along the separator x3 - x4
// const Index _x1_=0, _x2_=1, _x6_=2, _x5_=3, _x3_=4, _x4_=5; // const Index _x1_=0, _x2_=1, _x6_=2, _x5_=3, _x3_=4, _x4_=5;
// SymbolicFactorGraph fg1, fg2, fg3; // SymbolicFactorGraphOrdered fg1, fg2, fg3;
// fg1.push_factor(_x3_, _x4_); // fg1.push_factor(_x3_, _x4_);
// fg2.push_factor(_x1_, _x2_); // fg2.push_factor(_x1_, _x2_);
// fg2.push_factor(_x2_, _x3_); // fg2.push_factor(_x2_, _x3_);
@ -525,16 +525,16 @@ TEST( BayesTree, permute )
//// Ordering ordering2; ordering2 += _x1_, _x2_; //// Ordering ordering2; ordering2 += _x1_, _x2_;
//// Ordering ordering3; ordering3 += _x6_, _x5_; //// Ordering ordering3; ordering3 += _x6_, _x5_;
// //
// BayesNet<IndexConditional> bn1, bn2, bn3; // BayesNetOrdered<IndexConditionalOrdered> bn1, bn2, bn3;
// bn1 = *SymbolicSequentialSolver::EliminateUntil(fg1, _x4_+1); // bn1 = *SymbolicSequentialSolver::EliminateUntil(fg1, _x4_+1);
// bn2 = *SymbolicSequentialSolver::EliminateUntil(fg2, _x2_+1); // bn2 = *SymbolicSequentialSolver::EliminateUntil(fg2, _x2_+1);
// bn3 = *SymbolicSequentialSolver::EliminateUntil(fg3, _x5_+1); // bn3 = *SymbolicSequentialSolver::EliminateUntil(fg3, _x5_+1);
// //
// // insert child cliques // // insert child cliques
// SymbolicBayesTree actual; // SymbolicBayesTreeOrdered actual;
// list<SymbolicBayesTree::sharedClique> children; // list<SymbolicBayesTreeOrdered::sharedClique> children;
// SymbolicBayesTree::sharedClique r1 = actual.insert(bn2, children); // SymbolicBayesTreeOrdered::sharedClique r1 = actual.insert(bn2, children);
// SymbolicBayesTree::sharedClique r2 = actual.insert(bn3, children); // SymbolicBayesTreeOrdered::sharedClique r2 = actual.insert(bn3, children);
// //
// // insert root clique // // insert root clique
// children.push_back(r1); // children.push_back(r1);
@ -542,7 +542,7 @@ TEST( BayesTree, permute )
// actual.insert(bn1, children, true); // actual.insert(bn1, children, true);
// //
// // traditional way // // traditional way
// SymbolicFactorGraph fg; // SymbolicFactorGraphOrdered fg;
// fg.push_factor(_x3_, _x4_); // fg.push_factor(_x3_, _x4_);
// fg.push_factor(_x1_, _x2_); // fg.push_factor(_x1_, _x2_);
// fg.push_factor(_x2_, _x3_); // fg.push_factor(_x2_, _x3_);
@ -552,8 +552,8 @@ TEST( BayesTree, permute )
// fg.push_factor(_x6_, _x4_); // fg.push_factor(_x6_, _x4_);
// //
//// Ordering ordering; ordering += _x1_, _x2_, _x6_, _x5_, _x3_, _x4_; //// Ordering ordering; ordering += _x1_, _x2_, _x6_, _x5_, _x3_, _x4_;
// BayesNet<IndexConditional> bn(*SymbolicSequentialSolver(fg).eliminate()); // BayesNetOrdered<IndexConditionalOrdered> bn(*SymbolicSequentialSolver(fg).eliminate());
// SymbolicBayesTree expected(bn); // SymbolicBayesTreeOrdered expected(bn);
// CHECK(assert_equal(expected, actual)); // CHECK(assert_equal(expected, actual));
// //
//} //}

10
gtsam/inference/tests/testClusterTreeObsolete.cpp
View File

@ -21,17 +21,17 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/ClusterTree.h> #include <gtsam/inference/ClusterTreeOrdered.h>
using namespace gtsam; using namespace gtsam;
// explicit instantiation and typedef // explicit instantiation and typedef
namespace gtsam { template class ClusterTree<SymbolicFactorGraph>; } namespace gtsam { template class ClusterTreeOrdered<SymbolicFactorGraphOrdered>; }
typedef ClusterTree<SymbolicFactorGraph> SymbolicClusterTree; typedef ClusterTreeOrdered<SymbolicFactorGraphOrdered> SymbolicClusterTree;
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ClusterTree, constructor) { TEST(ClusterTreeOrdered, constructor) {
SymbolicClusterTree tree; SymbolicClusterTree tree;
} }

36
gtsam/inference/tests/testConditionalObsolete.cpp
View File

@ -20,76 +20,76 @@
using namespace boost::assign; using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/IndexConditional.h> #include <gtsam/inference/IndexConditionalOrdered.h>
#include <gtsam/inference/IndexFactor.h> #include <gtsam/inference/IndexFactorOrdered.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( IndexConditional, empty ) TEST( IndexConditionalOrdered, empty )
{ {
IndexConditional c0; IndexConditionalOrdered c0;
LONGS_EQUAL(0,c0.nrFrontals()) LONGS_EQUAL(0,c0.nrFrontals())
LONGS_EQUAL(0,c0.nrParents()) LONGS_EQUAL(0,c0.nrParents())
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( IndexConditional, noParents ) TEST( IndexConditionalOrdered, noParents )
{ {
IndexConditional c0(0); IndexConditionalOrdered c0(0);
LONGS_EQUAL(1,c0.nrFrontals()) LONGS_EQUAL(1,c0.nrFrontals())
LONGS_EQUAL(0,c0.nrParents()) LONGS_EQUAL(0,c0.nrParents())
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( IndexConditional, oneParents ) TEST( IndexConditionalOrdered, oneParents )
{ {
IndexConditional c0(0,1); IndexConditionalOrdered c0(0,1);
LONGS_EQUAL(1,c0.nrFrontals()) LONGS_EQUAL(1,c0.nrFrontals())
LONGS_EQUAL(1,c0.nrParents()) LONGS_EQUAL(1,c0.nrParents())
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( IndexConditional, twoParents ) TEST( IndexConditionalOrdered, twoParents )
{ {
IndexConditional c0(0,1,2); IndexConditionalOrdered c0(0,1,2);
LONGS_EQUAL(1,c0.nrFrontals()) LONGS_EQUAL(1,c0.nrFrontals())
LONGS_EQUAL(2,c0.nrParents()) LONGS_EQUAL(2,c0.nrParents())
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( IndexConditional, threeParents ) TEST( IndexConditionalOrdered, threeParents )
{ {
IndexConditional c0(0,1,2,3); IndexConditionalOrdered c0(0,1,2,3);
LONGS_EQUAL(1,c0.nrFrontals()) LONGS_EQUAL(1,c0.nrFrontals())
LONGS_EQUAL(3,c0.nrParents()) LONGS_EQUAL(3,c0.nrParents())
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( IndexConditional, fourParents ) TEST( IndexConditionalOrdered, fourParents )
{ {
vector<Index> parents; vector<Index> parents;
parents += 1,2,3,4; parents += 1,2,3,4;
IndexConditional c0(0,parents); IndexConditionalOrdered c0(0,parents);
LONGS_EQUAL(1,c0.nrFrontals()) LONGS_EQUAL(1,c0.nrFrontals())
LONGS_EQUAL(4,c0.nrParents()) LONGS_EQUAL(4,c0.nrParents())
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( IndexConditional, FromRange ) TEST( IndexConditionalOrdered, FromRange )
{ {
vector<Index> keys; vector<Index> keys;
keys += 1,2,3,4,5; keys += 1,2,3,4,5;
IndexConditional::shared_ptr c0(new IndexConditional(keys,2)); IndexConditionalOrdered::shared_ptr c0(new IndexConditionalOrdered(keys,2));
LONGS_EQUAL(2,c0->nrFrontals()) LONGS_EQUAL(2,c0->nrFrontals())
LONGS_EQUAL(3,c0->nrParents()) LONGS_EQUAL(3,c0->nrParents())
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( IndexConditional, equals ) TEST( IndexConditionalOrdered, equals )
{ {
IndexConditional c0(0, 1, 2), c1(0, 1, 2), c2(1, 2, 3), c3(3,4); IndexConditionalOrdered c0(0, 1, 2), c1(0, 1, 2), c2(1, 2, 3), c3(3,4);
CHECK(c0.equals(c1)); CHECK(c0.equals(c1));
CHECK(c1.equals(c0)); CHECK(c1.equals(c0));
CHECK(!c0.equals(c2)); CHECK(!c0.equals(c2));

50
gtsam/inference/tests/testEliminationTreeObsolete.cpp
View File

@ -19,33 +19,33 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/EliminationTree-inl.h> #include <gtsam/inference/EliminationTreeOrdered-inl.h>
#include <gtsam/inference/SymbolicSequentialSolver.h> #include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
class EliminationTreeTester { class EliminationTreeOrderedTester {
public: public:
// build hardcoded tree // build hardcoded tree
static SymbolicEliminationTree::shared_ptr buildHardcodedTree(const SymbolicFactorGraph& fg) { static SymbolicEliminationTreeOrdered::shared_ptr buildHardcodedTree(const SymbolicFactorGraphOrdered& fg) {
SymbolicEliminationTree::shared_ptr leaf0(new SymbolicEliminationTree); SymbolicEliminationTreeOrdered::shared_ptr leaf0(new SymbolicEliminationTreeOrdered);
leaf0->add(fg[0]); leaf0->add(fg[0]);
leaf0->add(fg[1]); leaf0->add(fg[1]);
SymbolicEliminationTree::shared_ptr node1(new SymbolicEliminationTree(1)); SymbolicEliminationTreeOrdered::shared_ptr node1(new SymbolicEliminationTreeOrdered(1));
node1->add(fg[2]); node1->add(fg[2]);
node1->add(leaf0); node1->add(leaf0);
SymbolicEliminationTree::shared_ptr node2(new SymbolicEliminationTree(2)); SymbolicEliminationTreeOrdered::shared_ptr node2(new SymbolicEliminationTreeOrdered(2));
node2->add(fg[3]); node2->add(fg[3]);
node2->add(node1); node2->add(node1);
SymbolicEliminationTree::shared_ptr leaf3(new SymbolicEliminationTree(3)); SymbolicEliminationTreeOrdered::shared_ptr leaf3(new SymbolicEliminationTreeOrdered(3));
leaf3->add(fg[4]); leaf3->add(fg[4]);
SymbolicEliminationTree::shared_ptr etree(new SymbolicEliminationTree(4)); SymbolicEliminationTreeOrdered::shared_ptr etree(new SymbolicEliminationTreeOrdered(4));
etree->add(leaf3); etree->add(leaf3);
etree->add(node2); etree->add(node2);
@ -53,20 +53,20 @@ public:
} }
}; };
TEST(EliminationTree, Create) TEST(EliminationTreeOrdered, Create)
{ {
// create example factor graph // create example factor graph
SymbolicFactorGraph fg; SymbolicFactorGraphOrdered fg;
fg.push_factor(0, 1); fg.push_factor(0, 1);
fg.push_factor(0, 2); fg.push_factor(0, 2);
fg.push_factor(1, 4); fg.push_factor(1, 4);
fg.push_factor(2, 4); fg.push_factor(2, 4);
fg.push_factor(3, 4); fg.push_factor(3, 4);
SymbolicEliminationTree::shared_ptr expected = EliminationTreeTester::buildHardcodedTree(fg); SymbolicEliminationTreeOrdered::shared_ptr expected = EliminationTreeOrderedTester::buildHardcodedTree(fg);
// Build from factor graph // Build from factor graph
SymbolicEliminationTree::shared_ptr actual = SymbolicEliminationTree::Create(fg); SymbolicEliminationTreeOrdered::shared_ptr actual = SymbolicEliminationTreeOrdered::Create(fg);
CHECK(assert_equal(*expected,*actual)); CHECK(assert_equal(*expected,*actual));
} }
@ -76,18 +76,18 @@ TEST(EliminationTree, Create)
// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4) // graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(EliminationTree, eliminate ) TEST(EliminationTreeOrdered, eliminate )
{ {
// create expected Chordal bayes Net // create expected Chordal bayes Net
SymbolicBayesNet expected; SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditional>(4)); expected.push_front(boost::make_shared<IndexConditionalOrdered>(4));
expected.push_front(boost::make_shared<IndexConditional>(3,4)); expected.push_front(boost::make_shared<IndexConditionalOrdered>(3,4));
expected.push_front(boost::make_shared<IndexConditional>(2,4)); expected.push_front(boost::make_shared<IndexConditionalOrdered>(2,4));
expected.push_front(boost::make_shared<IndexConditional>(1,2,4)); expected.push_front(boost::make_shared<IndexConditionalOrdered>(1,2,4));
expected.push_front(boost::make_shared<IndexConditional>(0,1,2)); expected.push_front(boost::make_shared<IndexConditionalOrdered>(0,1,2));
// Create factor graph // Create factor graph
SymbolicFactorGraph fg; SymbolicFactorGraphOrdered fg;
fg.push_factor(0, 1); fg.push_factor(0, 1);
fg.push_factor(0, 2); fg.push_factor(0, 2);
fg.push_factor(1, 4); fg.push_factor(1, 4);
@ -95,20 +95,20 @@ TEST(EliminationTree, eliminate )
fg.push_factor(3, 4); fg.push_factor(3, 4);
// eliminate // eliminate
SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate(); SymbolicBayesNetOrdered actual = *SymbolicSequentialSolver(fg).eliminate();
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(EliminationTree, disconnected_graph) { TEST(EliminationTreeOrdered, disconnected_graph) {
SymbolicFactorGraph fg; SymbolicFactorGraphOrdered fg;
fg.push_factor(0, 1); fg.push_factor(0, 1);
fg.push_factor(0, 2); fg.push_factor(0, 2);
fg.push_factor(1, 2); fg.push_factor(1, 2);
fg.push_factor(3, 4); fg.push_factor(3, 4);
CHECK_EXCEPTION(SymbolicEliminationTree::Create(fg), DisconnectedGraphException); CHECK_EXCEPTION(SymbolicEliminationTreeOrdered::Create(fg), DisconnectedGraphException);
} }
/* ************************************************************************* */ /* ************************************************************************* */

16
gtsam/inference/tests/testFactorGraphObsolete.cpp
View File

@ -26,16 +26,16 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/IndexConditional.h> #include <gtsam/inference/IndexConditionalOrdered.h>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraphOrdered.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
TEST(FactorGraph, eliminateFrontals) { TEST(FactorGraphOrdered, eliminateFrontals) {
SymbolicFactorGraph sfgOrig; SymbolicFactorGraphOrdered sfgOrig;
sfgOrig.push_factor(0,1); sfgOrig.push_factor(0,1);
sfgOrig.push_factor(0,2); sfgOrig.push_factor(0,2);
sfgOrig.push_factor(1,3); sfgOrig.push_factor(1,3);
@ -43,15 +43,15 @@ TEST(FactorGraph, eliminateFrontals) {
sfgOrig.push_factor(2,3); sfgOrig.push_factor(2,3);
sfgOrig.push_factor(4,5); sfgOrig.push_factor(4,5);
IndexConditional::shared_ptr actualCond; IndexConditionalOrdered::shared_ptr actualCond;
SymbolicFactorGraph actualSfg; SymbolicFactorGraphOrdered actualSfg;
boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2); boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2);
vector<Index> condIndices; vector<Index> condIndices;
condIndices += 0,1,2,3,4; condIndices += 0,1,2,3,4;
IndexConditional expectedCond(condIndices, 2); IndexConditionalOrdered expectedCond(condIndices, 2);
SymbolicFactorGraph expectedSfg; SymbolicFactorGraphOrdered expectedSfg;
expectedSfg.push_factor(2,3); expectedSfg.push_factor(2,3);
expectedSfg.push_factor(4,5); expectedSfg.push_factor(4,5);
expectedSfg.push_factor(2,3,4); expectedSfg.push_factor(2,3,4);

10
gtsam/inference/tests/testISAM.cpp
View File

@ -21,15 +21,15 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/BayesNet.h> #include <gtsam/inference/BayesNetOrdered.h>
#include <gtsam/inference/IndexConditional.h> #include <gtsam/inference/IndexConditionalOrdered.h>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/ISAM.h> #include <gtsam/inference/ISAMOrdered.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
typedef ISAM<IndexConditional> SymbolicISAM; typedef ISAMOrdered<IndexConditionalOrdered> SymbolicISAM;
/* ************************************************************************* */ /* ************************************************************************* */
// Some numbers that should be consistent among all smoother tests // Some numbers that should be consistent among all smoother tests

18
gtsam/inference/tests/testInference.cpp
View File

@ -18,22 +18,22 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/inference.h> #include <gtsam/inference/inferenceOrdered.h>
#include <gtsam/inference/VariableIndex.h> #include <gtsam/inference/VariableIndexOrdered.h>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraphOrdered.h>
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
TEST(inference, UnobservedVariables) { TEST(inference, UnobservedVariables) {
SymbolicFactorGraph sfg; SymbolicFactorGraphOrdered sfg;
// Create a factor graph that skips some variables // Create a factor graph that skips some variables
sfg.push_factor(0,1); sfg.push_factor(0,1);
sfg.push_factor(1,3); sfg.push_factor(1,3);
sfg.push_factor(3,5); sfg.push_factor(3,5);
VariableIndex variableIndex(sfg); VariableIndexOrdered variableIndex(sfg);
// Computes a permutation with known variables first, skipped variables last // Computes a permutation with known variables first, skipped variables last
// Actual 0 1 3 5 2 4 // Actual 0 1 3 5 2 4
@ -50,7 +50,7 @@ TEST(inference, UnobservedVariables) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(inference, constrained_ordering) { TEST(inference, constrained_ordering) {
SymbolicFactorGraph sfg; SymbolicFactorGraphOrdered sfg;
// create graph with wanted variable set = 2, 4 // create graph with wanted variable set = 2, 4
sfg.push_factor(0,1); sfg.push_factor(0,1);
@ -59,7 +59,7 @@ TEST(inference, constrained_ordering) {
sfg.push_factor(3,4); sfg.push_factor(3,4);
sfg.push_factor(4,5); sfg.push_factor(4,5);
VariableIndex variableIndex(sfg); VariableIndexOrdered variableIndex(sfg);
// unconstrained version // unconstrained version
Permutation::shared_ptr actUnconstrained(inference::PermutationCOLAMD(variableIndex)); Permutation::shared_ptr actUnconstrained(inference::PermutationCOLAMD(variableIndex));
@ -83,7 +83,7 @@ TEST(inference, constrained_ordering) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(inference, grouped_constrained_ordering) { TEST(inference, grouped_constrained_ordering) {
SymbolicFactorGraph sfg; SymbolicFactorGraphOrdered sfg;
// create graph with constrained groups: // create graph with constrained groups:
// 1: 2, 4 // 1: 2, 4
@ -94,7 +94,7 @@ TEST(inference, grouped_constrained_ordering) {
sfg.push_factor(3,4); sfg.push_factor(3,4);
sfg.push_factor(4,5); sfg.push_factor(4,5);
VariableIndex variableIndex(sfg); VariableIndexOrdered variableIndex(sfg);
// constrained version - push one set to the end // constrained version - push one set to the end
FastMap<size_t, int> constraints; FastMap<size_t, int> constraints;

28
gtsam/inference/tests/testJunctionTreeObsolete.cpp
View File

@ -24,27 +24,27 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/JunctionTree.h> #include <gtsam/inference/JunctionTreeOrdered.h>
#include <gtsam/inference/ClusterTree.h> #include <gtsam/inference/ClusterTreeOrdered.h>
#include <gtsam/inference/JunctionTree.h> #include <gtsam/inference/JunctionTreeOrdered.h>
#include <gtsam/inference/IndexFactor.h> #include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/SymbolicSequentialSolver.h> #include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
typedef JunctionTree<SymbolicFactorGraph> SymbolicJunctionTree; typedef JunctionTreeOrdered<SymbolicFactorGraphOrdered> SymbolicJunctionTree;
/* ************************************************************************* * /* ************************************************************************* *
* x1 - x2 - x3 - x4 * x1 - x2 - x3 - x4
* x3 x4 * x3 x4
* x2 x1 : x3 * x2 x1 : x3
****************************************************************************/ ****************************************************************************/
TEST( JunctionTree, constructor ) TEST( JunctionTreeOrdered, constructor )
{ {
const Index x2=0, x1=1, x3=2, x4=3; const Index x2=0, x1=1, x3=2, x4=3;
SymbolicFactorGraph fg; SymbolicFactorGraphOrdered fg;
fg.push_factor(x2,x1); fg.push_factor(x2,x1);
fg.push_factor(x2,x3); fg.push_factor(x2,x3);
fg.push_factor(x3,x4); fg.push_factor(x3,x4);
@ -71,19 +71,19 @@ TEST( JunctionTree, constructor )
* x3 x4 * x3 x4
* x2 x1 : x3 * x2 x1 : x3
****************************************************************************/ ****************************************************************************/
TEST( JunctionTree, eliminate) TEST( JunctionTreeOrdered, eliminate)
{ {
const Index x2=0, x1=1, x3=2, x4=3; const Index x2=0, x1=1, x3=2, x4=3;
SymbolicFactorGraph fg; SymbolicFactorGraphOrdered fg;
fg.push_factor(x2,x1); fg.push_factor(x2,x1);
fg.push_factor(x2,x3); fg.push_factor(x2,x3);
fg.push_factor(x3,x4); fg.push_factor(x3,x4);
SymbolicJunctionTree jt(fg); SymbolicJunctionTree jt(fg);
SymbolicBayesTree::sharedClique actual = jt.eliminate(&EliminateSymbolic); SymbolicBayesTreeOrdered::sharedClique actual = jt.eliminate(&EliminateSymbolic);
BayesNet<IndexConditional> bn(*SymbolicSequentialSolver(fg).eliminate()); BayesNetOrdered<IndexConditionalOrdered> bn(*SymbolicSequentialSolver(fg).eliminate());
SymbolicBayesTree expected(bn); SymbolicBayesTreeOrdered expected(bn);
// cout << "BT from JT:\n"; // cout << "BT from JT:\n";
// actual->printTree(""); // actual->printTree("");

2
gtsam/inference/tests/testPermutationObsolete.cpp
View File

@ -18,7 +18,7 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/inference/Permutation.h> #include <gtsam/inference/PermutationOrdered.h>
#include <vector> #include <vector>

32
gtsam/inference/tests/testSerializationInference.cpp
View File

@ -16,9 +16,9 @@
* @date Feb 7, 2012 * @date Feb 7, 2012
*/ */
#include <gtsam/inference/IndexConditional.h> #include <gtsam/inference/IndexConditionalOrdered.h>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/BayesTree.h> #include <gtsam/inference/BayesTreeOrdered.h>
#include <gtsam/base/serializationTestHelpers.h> #include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -30,7 +30,7 @@ using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */ /* ************************************************************************* */
TEST (Serialization, symbolic_graph) { TEST (Serialization, symbolic_graph) {
// construct expected symbolic graph // construct expected symbolic graph
SymbolicFactorGraph sfg; SymbolicFactorGraphOrdered sfg;
sfg.push_factor(0); sfg.push_factor(0);
sfg.push_factor(0,1); sfg.push_factor(0,1);
sfg.push_factor(0,2); sfg.push_factor(0,2);
@ -43,11 +43,11 @@ TEST (Serialization, symbolic_graph) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST (Serialization, symbolic_bn) { TEST (Serialization, symbolic_bn) {
IndexConditional::shared_ptr x2(new IndexConditional(1, 2, 0)); IndexConditionalOrdered::shared_ptr x2(new IndexConditionalOrdered(1, 2, 0));
IndexConditional::shared_ptr l1(new IndexConditional(2, 0)); IndexConditionalOrdered::shared_ptr l1(new IndexConditionalOrdered(2, 0));
IndexConditional::shared_ptr x1(new IndexConditional(0)); IndexConditionalOrdered::shared_ptr x1(new IndexConditionalOrdered(0));
SymbolicBayesNet sbn; SymbolicBayesNetOrdered sbn;
sbn.push_back(x2); sbn.push_back(x2);
sbn.push_back(l1); sbn.push_back(l1);
sbn.push_back(x1); sbn.push_back(x1);
@ -59,15 +59,15 @@ TEST (Serialization, symbolic_bn) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST (Serialization, symbolic_bayes_tree ) { TEST (Serialization, symbolic_bayes_tree ) {
typedef BayesTree<IndexConditional> SymbolicBayesTree; typedef BayesTreeOrdered<IndexConditionalOrdered> SymbolicBayesTree;
static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5; static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5;
IndexConditional::shared_ptr IndexConditionalOrdered::shared_ptr
B(new IndexConditional(_B_)), B(new IndexConditionalOrdered(_B_)),
L(new IndexConditional(_L_, _B_)), L(new IndexConditionalOrdered(_L_, _B_)),
E(new IndexConditional(_E_, _L_, _B_)), E(new IndexConditionalOrdered(_E_, _L_, _B_)),
S(new IndexConditional(_S_, _L_, _B_)), S(new IndexConditionalOrdered(_S_, _L_, _B_)),
T(new IndexConditional(_T_, _E_, _L_)), T(new IndexConditionalOrdered(_T_, _E_, _L_)),
X(new IndexConditional(_X_, _E_)); X(new IndexConditionalOrdered(_X_, _E_));
// Bayes Tree for Asia example // Bayes Tree for Asia example
SymbolicBayesTree bayesTree; SymbolicBayesTree bayesTree;

Some files were not shown because too many files have changed in this diff Show More