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

52
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -22,11 +22,11 @@
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/OrderingOrdered.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNetOrdered.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/geometry/Point2.h>
@ -38,10 +38,10 @@ int main() {
// [code below basically does SRIF with Cholesky]
// Create a factor graph to perform the inference
GaussianFactorGraph::shared_ptr linearFactorGraph(new GaussianFactorGraph);
GaussianFactorGraphOrdered::shared_ptr linearFactorGraph(new GaussianFactorGraphOrdered);
// Create the desired ordering
Ordering::shared_ptr ordering(new Ordering);
OrderingOrdered::shared_ptr ordering(new OrderingOrdered);
// Create a structure to hold the linearization points
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) )
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
VectorValues result = optimize(*linearBayesNet);
VectorValuesOrdered result = optimize(*linearBayesNet);
Point2 x1_predict = linearizationPoints.at<Point2>(x1).retract(result[ordering->at(x1)]);
x1_predict.print("X1 Predict");
@ -123,7 +123,7 @@ int main() {
// 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
// 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'')
// = || F*dx1'' - (b' - F(dx1' - dx1'')) ||^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->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));
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
ordering = OrderingOrdered::shared_ptr(new OrderingOrdered);
ordering->insert(x1, 0);
// 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
// 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
// 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
const GaussianConditional::shared_ptr& cg1 = linearBayesNet->back();
const GaussianConditionalOrdered::shared_ptr& cg1 = linearBayesNet->back();
assert(cg1->nrFrontals() == 1);
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());
// Create a key for the new state
Symbol x2('x',2);
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
ordering = OrderingOrdered::shared_ptr(new OrderingOrdered);
ordering->insert(x1, 0);
ordering->insert(x2, 1);
@ -243,17 +243,17 @@ int main() {
// Now add the next measurement
// 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
const GaussianConditional::shared_ptr& cg2 = linearBayesNet->back();
const GaussianConditionalOrdered::shared_ptr& cg2 = linearBayesNet->back();
assert(cg2->nrFrontals() == 1);
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());
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
ordering = OrderingOrdered::shared_ptr(new OrderingOrdered);
ordering->insert(x2, 0);
// And update using z2 ...
@ -290,20 +290,20 @@ int main() {
// Wash, rinse, repeat for a third time 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
const GaussianConditional::shared_ptr& cg3 = linearBayesNet->back();
const GaussianConditionalOrdered::shared_ptr& cg3 = linearBayesNet->back();
assert(cg3->nrFrontals() == 1);
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());
// Create a key for the new state
Symbol x3('x',3);
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
ordering = OrderingOrdered::shared_ptr(new OrderingOrdered);
ordering->insert(x2, 0);
ordering->insert(x3, 1);
@ -332,17 +332,17 @@ int main() {
// Now add the next measurement
// 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
const GaussianConditional::shared_ptr& cg4 = linearBayesNet->back();
const GaussianConditionalOrdered::shared_ptr& cg4 = linearBayesNet->back();
assert(cg4->nrFrontals() == 1);
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());
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
ordering = OrderingOrdered::shared_ptr(new OrderingOrdered);
ordering->insert(x3, 0);
// 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 {
return IndexFactor::equals(other, tol) && Potentials::equals(other, tol);
return IndexFactorOrdered::equals(other, tol) && Potentials::equals(other, tol);
}
/* ************************************************************************* */
void DecisionTreeFactor::print(const string& s,
const IndexFormatter& formatter) const {
cout << s;
IndexFactor::print("IndexFactor:",formatter);
IndexFactorOrdered::print("IndexFactor:",formatter);
Potentials::print("Potentials:",formatter);
}

4
gtsam/discrete/DiscreteBayesNet.cpp
View File

@ -18,13 +18,13 @@
#include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/BayesNetOrdered-inl.h>
#include <boost/make_shared.hpp>
namespace gtsam {
// 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) {

4
gtsam/discrete/DiscreteBayesNet.h
View File

@ -20,12 +20,12 @@
#include <vector>
#include <map>
#include <boost/shared_ptr.hpp>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/BayesNetOrdered.h>
#include <gtsam/discrete/DiscreteConditional.h>
namespace gtsam {
typedef BayesNet<DiscreteConditional> DiscreteBayesNet;
typedef BayesNetOrdered<DiscreteConditional> DiscreteBayesNet;
/** Add a DiscreteCondtional */
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,
const DecisionTreeFactor& f) :
IndexConditional(f.keys(), nrFrontals), Potentials(
IndexConditionalOrdered(f.keys(), nrFrontals), Potentials(
f / (*f.sum(nrFrontals))) {
}
/* ******************************************************************************** */
DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
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) {
if (ISDEBUG("DiscreteConditional::DiscreteConditional"))
cout << (firstFrontalKey()) << endl; //TODO Print all keys
@ -52,20 +52,20 @@ namespace gtsam {
/* ******************************************************************************** */
DiscreteConditional::DiscreteConditional(const Signature& signature) :
IndexConditional(signature.indices(), 1), Potentials(
IndexConditionalOrdered(signature.indices(), 1), Potentials(
signature.discreteKeysParentsFirst(), signature.cpt()) {
}
/* ******************************************************************************** */
void DiscreteConditional::print(const std::string& s, const IndexFormatter& formatter) const {
std::cout << s << std::endl;
IndexConditional::print(s, formatter);
IndexConditionalOrdered::print(s, formatter);
Potentials::print(s);
}
/* ******************************************************************************** */
bool DiscreteConditional::equals(const DiscreteConditional& other, double tol) const {
return IndexConditional::equals(other, tol)
return IndexConditionalOrdered::equals(other, tol)
&& Potentials::equals(other, tol);
}
@ -197,7 +197,7 @@ namespace gtsam {
/* ******************************************************************************** */
void DiscreteConditional::permuteWithInverse(const Permutation& inversePermutation){
IndexConditional::permuteWithInverse(inversePermutation);
IndexConditionalOrdered::permuteWithInverse(inversePermutation);
Potentials::permuteWithInverse(inversePermutation);
}

6
gtsam/discrete/DiscreteConditional.h
View File

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

18
gtsam/discrete/DiscreteFactor.h
View File

@ -19,7 +19,7 @@
#pragma once
#include <gtsam/discrete/Assignment.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/IndexFactorOrdered.h>
namespace gtsam {
@ -30,7 +30,7 @@ namespace gtsam {
* Base class for discrete probabilistic factors
* The most general one is the derived DecisionTreeFactor
*/
class GTSAM_EXPORT DiscreteFactor: public IndexFactor {
class GTSAM_EXPORT DiscreteFactor: public IndexFactorOrdered {
public:
@ -47,23 +47,23 @@ namespace gtsam {
/// Construct n-way factor
DiscreteFactor(const std::vector<Index>& js) :
IndexFactor(js) {
IndexFactorOrdered(js) {
}
/// Construct unary factor
DiscreteFactor(Index j) :
IndexFactor(j) {
IndexFactorOrdered(j) {
}
/// Construct binary factor
DiscreteFactor(Index j1, Index j2) :
IndexFactor(j1, j2) {
IndexFactorOrdered(j1, j2) {
}
/// construct from container
template<class KeyIterator>
DiscreteFactor(KeyIterator beginKey, KeyIterator endKey) :
IndexFactor(beginKey, endKey) {
IndexFactorOrdered(beginKey, endKey) {
}
public:
@ -85,7 +85,7 @@ namespace gtsam {
virtual void print(const std::string& s = "DiscreteFactor\n",
const IndexFormatter& formatter
=DefaultIndexFormatter) const {
IndexFactor::print(s,formatter);
IndexFactorOrdered::print(s,formatter);
}
/// @}
@ -105,14 +105,14 @@ namespace gtsam {
* to already be inverted.
*/
virtual void permuteWithInverse(const Permutation& inversePermutation){
IndexFactor::permuteWithInverse(inversePermutation);
IndexFactorOrdered::permuteWithInverse(inversePermutation);
}
/**
* Apply a reduction, which is a remapping of variable indices.
*/
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
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/inference/EliminationTreeOrdered-inl.h>
#include <boost/make_shared.hpp>
namespace gtsam {
// Explicitly instantiate so we don't have to include everywhere
template class FactorGraph<DiscreteFactor> ;
template class EliminationTree<DiscreteFactor> ;
template class FactorGraphOrdered<DiscreteFactor> ;
template class EliminationTreeOrdered<DiscreteFactor> ;
/* ************************************************************************* */
DiscreteFactorGraph::DiscreteFactorGraph() {
@ -34,8 +34,8 @@ namespace gtsam {
/* ************************************************************************* */
DiscreteFactorGraph::DiscreteFactorGraph(
const BayesNet<DiscreteConditional>& bayesNet) :
FactorGraph<DiscreteFactor>(bayesNet) {
const BayesNetOrdered<DiscreteConditional>& bayesNet) :
FactorGraphOrdered<DiscreteFactor>(bayesNet) {
}
/* ************************************************************************* */
@ -95,7 +95,7 @@ namespace gtsam {
/* ************************************************************************* */
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
gttic(product);

10
gtsam/discrete/DiscreteFactorGraph.h
View File

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

4
gtsam/discrete/DiscreteMarginals.h
View File

@ -32,7 +32,7 @@ namespace gtsam {
protected:
BayesTree<DiscreteConditional> bayesTree_;
BayesTreeOrdered<DiscreteConditional> bayesTree_;
public:
@ -40,7 +40,7 @@ namespace gtsam {
* @param graph The factor graph defining the full joint density on all variables.
*/
DiscreteMarginals(const DiscreteFactorGraph& graph) {
typedef JunctionTree<DiscreteFactorGraph> DiscreteJT;
typedef JunctionTreeOrdered<DiscreteFactorGraph> DiscreteJT;
GenericMultifrontalSolver<DiscreteFactor, DiscreteJT> solver(graph);
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
* tree, which already does some of the work of elimination.
*/
DiscreteSequentialSolver(const FactorGraph<DiscreteFactor>& factorGraph) :
DiscreteSequentialSolver(const FactorGraphOrdered<DiscreteFactor>& factorGraph) :
Base(factorGraph) {
}
@ -57,12 +57,12 @@ namespace gtsam {
* is the fastest.
*/
DiscreteSequentialSolver(
const FactorGraph<DiscreteFactor>::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex) :
const FactorGraphOrdered<DiscreteFactor>::shared_ptr& factorGraph,
const VariableIndexOrdered::shared_ptr& variableIndex) :
Base(factorGraph, variableIndex) {
}
const EliminationTree<DiscreteFactor>& eliminationTree() const {
const EliminationTreeOrdered<DiscreteFactor>& eliminationTree() const {
return *eliminationTree_;
}
@ -70,7 +70,7 @@ namespace gtsam {
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
BayesNet<DiscreteConditional>::shared_ptr eliminate() const {
BayesNetOrdered<DiscreteConditional>::shared_ptr eliminate() const {
return Base::eliminate(&EliminateDiscrete);
}

2
gtsam/discrete/Potentials.h
View File

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

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

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

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

@ -60,7 +60,7 @@ TEST_UNSAFE( DiscreteFactorGraph, debugScheduler) {
//
DiscreteSequentialSolver solver(graph);
// solver.print("solver:");
EliminationTree<DiscreteFactor> eliminationTree = solver.eliminationTree();
EliminationTreeOrdered<DiscreteFactor> eliminationTree = solver.eliminationTree();
// eliminationTree.print("Elimination tree: ");
eliminationTree.eliminate(EliminateDiscrete);
// solver.optimize();
@ -155,7 +155,7 @@ TEST( DiscreteFactorGraph, test)
// GTSAM_PRINT(expected);
// Test elimination tree
const EliminationTree<DiscreteFactor>& etree = solver.eliminationTree();
const EliminationTreeOrdered<DiscreteFactor>& etree = solver.eliminationTree();
DiscreteBayesNet::shared_ptr actual = etree.eliminate(&EliminateDiscrete);
EXPECT(assert_equal(expected, *actual));
@ -221,16 +221,16 @@ TEST( DiscreteFactorGraph, testMPE_Darwiche09book_p244)
// 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
typedef JunctionTree<DiscreteFactorGraph> JT;
typedef JunctionTreeOrdered<DiscreteFactorGraph> JT;
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");
EXPECT_LONGS_EQUAL(2,bayesTree->size());
#ifdef OLD
// Create the elimination tree manually
VariableIndex structure(graph);
typedef EliminationTree<DiscreteFactor> ETree;
VariableIndexOrdered structure(graph);
typedef EliminationTreeOrdered<DiscreteFactor> ETree;
ETree::shared_ptr eTree = ETree::Create(graph, structure);
//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[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");
typedef JunctionTree<DiscreteFactorGraph> JT;
typedef JunctionTreeOrdered<DiscreteFactorGraph> JT;
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");
typedef BayesTreeClique<DiscreteConditional> Clique;

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

@ -18,8 +18,8 @@
#pragma once
#include <gtsam/inference/FactorGraphUnordered-inst.h>
#include <gtsam/inference/BayesNetUnordered.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/inference/BayesNet.h>
#include <boost/foreach.hpp>
#include <fstream>
@ -28,14 +28,14 @@ namespace gtsam {
/* ************************************************************************* */
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);
}
/* ************************************************************************* */
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());
of << "digraph G{\n";

316
gtsam/inference/BayesNet.h
View File

@ -1,244 +1,72 @@
/* ----------------------------------------------------------------------------
* 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/Permutation.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 BayesNet {
public:
typedef typename boost::shared_ptr<BayesNet<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 */
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>
/* ----------------------------------------------------------------------------
* 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/FactorGraph.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 BayesNet : public FactorGraph<CONDITIONAL> {
private:
typedef FactorGraph<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 */
BayesNet() {};
/** Construct from iterator over conditionals */
template<typename ITERATOR>
BayesNet(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;
};
}

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

@ -34,7 +34,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::print(const std::string& s,
void BayesNetOrdered<CONDITIONAL>::print(const std::string& s,
const IndexFormatter& formatter) const {
std::cout << s;
BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_)
@ -43,7 +43,7 @@ namespace gtsam {
/* ************************************************************************* */
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())
return false;
return std::equal(conditionals_.begin(), conditionals_.end(),
@ -52,7 +52,7 @@ namespace gtsam {
/* ************************************************************************* */
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();
size_t max_size = 0;
@ -69,7 +69,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::saveGraph(const std::string &s,
void BayesNetOrdered<CONDITIONAL>::saveGraph(const std::string &s,
const IndexFormatter& indexFormatter) const {
std::ofstream of(s.c_str());
of << "digraph G{\n";
@ -88,7 +88,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::const_iterator BayesNet<CONDITIONAL>::find(
typename BayesNetOrdered<CONDITIONAL>::const_iterator BayesNetOrdered<CONDITIONAL>::find(
Index key) const {
for (const_iterator it = begin(); it != end(); ++it)
if (std::find((*it)->beginFrontals(), (*it)->endFrontals(), key)
@ -99,7 +99,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::iterator BayesNet<CONDITIONAL>::find(
typename BayesNetOrdered<CONDITIONAL>::iterator BayesNetOrdered<CONDITIONAL>::find(
Index key) {
for (iterator it = begin(); it != end(); ++it)
if (std::find((*it)->beginFrontals(), (*it)->endFrontals(), key)
@ -110,7 +110,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::permuteWithInverse(
void BayesNetOrdered<CONDITIONAL>::permuteWithInverse(
const Permutation& inversePermutation) {
BOOST_FOREACH(sharedConditional conditional, conditionals_) {
conditional->permuteWithInverse(inversePermutation);
@ -119,21 +119,21 @@ namespace gtsam {
/* ************************************************************************* */
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_)
push_back(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_)
push_front(conditional);
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::popLeaf(iterator conditional) {
void BayesNetOrdered<CONDITIONAL>::popLeaf(iterator conditional) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
BOOST_FOREACH(typename CONDITIONAL::shared_ptr checkConditional, conditionals_) {
BOOST_FOREACH(Index key, (*conditional)->frontals()) {
@ -148,7 +148,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
FastList<Index> BayesNet<CONDITIONAL>::ordering() const {
FastList<Index> BayesNetOrdered<CONDITIONAL>::ordering() const {
FastList<Index> ord;
BOOST_FOREACH(sharedConditional conditional,conditionals_)
ord.insert(ord.begin(), conditional->beginFrontals(),
@ -174,7 +174,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::sharedConditional BayesNet<CONDITIONAL>::operator[](
typename BayesNetOrdered<CONDITIONAL>::sharedConditional BayesNetOrdered<CONDITIONAL>::operator[](
Index key) const {
BOOST_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) {
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
#include <gtsam/inference/BayesTreeUnordered.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/base/timing.h>
@ -34,8 +34,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::CliqueData
BayesTreeUnordered<CLIQUE>::getCliqueData() const {
typename BayesTree<CLIQUE>::CliqueData
BayesTree<CLIQUE>::getCliqueData() const {
CliqueData data;
BOOST_FOREACH(const sharedClique& root, roots_)
getCliqueData(data, root);
@ -44,7 +44,7 @@ namespace gtsam {
/* ************************************************************************* */
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.separatorSizes.push_back(clique->conditional()->nrParents());
BOOST_FOREACH(sharedClique c, clique->children) {
@ -54,7 +54,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
size_t BayesTreeUnordered<CLIQUE>::numCachedSeparatorMarginals() const {
size_t BayesTree<CLIQUE>::numCachedSeparatorMarginals() const {
size_t count = 0;
BOOST_FOREACH(const sharedClique& root, roots_)
count += root->numCachedSeparatorMarginals();
@ -63,7 +63,7 @@ namespace gtsam {
/* ************************************************************************* */
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!");
std::ofstream of(s.c_str());
of<< "digraph G{\n";
@ -75,7 +75,7 @@ namespace gtsam {
/* ************************************************************************* */
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;
bool first = true;
std::stringstream out;
@ -111,7 +111,7 @@ namespace gtsam {
/* ************************************************************************* */
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
<< "avg Conditional Size: " << avgConditionalSize << std::endl
<< "max Conditional Size: " << maxConditionalSize << std::endl
@ -121,8 +121,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::CliqueStats
BayesTreeUnordered<CLIQUE>::CliqueData::getStats() const
typename BayesTree<CLIQUE>::CliqueStats
BayesTree<CLIQUE>::CliqueData::getStats() const
{
CliqueStats stats;
@ -149,7 +149,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
size_t BayesTreeUnordered<CLIQUE>::size() const {
size_t BayesTree<CLIQUE>::size() const {
size_t size = 0;
BOOST_FOREACH(const sharedClique& clique, roots_)
size += clique->treeSize();
@ -158,7 +158,7 @@ namespace gtsam {
/* ************************************************************************* */
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())
nodes_[j] = clique;
if (parent_clique != NULL) {
@ -171,7 +171,7 @@ namespace gtsam {
/* ************************************************************************* */
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)
{
sharedClique new_clique(new Clique(conditional));
@ -186,7 +186,7 @@ namespace gtsam {
/* ************************************************************************* */
namespace {
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_);
return 0;
}
@ -194,7 +194,7 @@ namespace gtsam {
/* ************************************************************************* */
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
int data = 0; // Unused
@ -203,7 +203,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
BayesTreeUnordered<CLIQUE>::BayesTreeUnordered(const This& other) {
BayesTree<CLIQUE>::BayesTree(const This& other) {
*this = other;
}
@ -224,7 +224,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
BayesTreeUnordered<CLIQUE>& BayesTreeUnordered<CLIQUE>::operator=(const This& other) {
BayesTree<CLIQUE>& BayesTree<CLIQUE>::operator=(const This& other) {
this->clear();
boost::shared_ptr<Clique> rootContainer = boost::make_shared<Clique>();
treeTraversal::DepthFirstForest(other, rootContainer, BayesTreeCloneForestVisitorPre<Clique>);
@ -237,7 +237,7 @@ namespace gtsam {
/* ************************************************************************* */
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;
treeTraversal::PrintForest(*this, s, keyFormatter);
}
@ -246,8 +246,8 @@ namespace gtsam {
// binary predicate to test equality of a pair for use in equals
template<class CLIQUE>
bool check_sharedCliques(
const std::pair<Key, typename BayesTreeUnordered<CLIQUE>::sharedClique>& v1,
const std::pair<Key, typename BayesTreeUnordered<CLIQUE>::sharedClique>& v2
const std::pair<Key, typename BayesTree<CLIQUE>::sharedClique>& v1,
const std::pair<Key, typename BayesTree<CLIQUE>::sharedClique>& v2
) {
return v1.first == v2.first &&
((!v1.second && !v2.second) || (v1.second && v2.second && v1.second->equals(*v2.second)));
@ -255,7 +255,7 @@ namespace gtsam {
/* ************************************************************************* */
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() &&
std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CLIQUE>);
}
@ -263,7 +263,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
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());
assert(lowestOrderedParent != parents.end());
return *lowestOrderedParent;
@ -271,18 +271,18 @@ namespace gtsam {
/* ************************************************************************* */
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
BOOST_FOREACH(const Key& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; }
// Fill index for each child
typedef typename BayesTreeUnordered<CLIQUE>::sharedClique sharedClique;
typedef typename BayesTree<CLIQUE>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& child, subtree->children) {
fillNodesIndex(child); }
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::insertRoot(const sharedClique& subtree) {
void BayesTree<CLIQUE>::insertRoot(const sharedClique& subtree) {
roots_.push_back(subtree); // Add to roots
fillNodesIndex(subtree); // Populate nodes index
}
@ -291,8 +291,8 @@ namespace gtsam {
// First finds clique marginal then marginalizes that
/* ************************************************************************* */
template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::sharedConditional
BayesTreeUnordered<CLIQUE>::marginalFactor(Key j, const Eliminate& function) const
typename BayesTree<CLIQUE>::sharedConditional
BayesTree<CLIQUE>::marginalFactor(Key j, const Eliminate& function) const
{
gttic(BayesTree_marginalFactor);
@ -304,7 +304,7 @@ namespace gtsam {
// Now, marginalize out everything that is not variable j
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
return marginalBN.front();
@ -314,8 +314,8 @@ namespace gtsam {
// Find two cliques, their joint, then marginalizes
/* ************************************************************************* */
template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::sharedFactorGraph
BayesTreeUnordered<CLIQUE>::joint(Key j1, Key j2, const Eliminate& function) const
typename BayesTree<CLIQUE>::sharedFactorGraph
BayesTree<CLIQUE>::joint(Key j1, Key j2, const Eliminate& function) const
{
gttic(BayesTree_joint);
return boost::make_shared<FactorGraphType>(*jointBayesNet(j1, j2, function));
@ -323,8 +323,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::sharedBayesNet
BayesTreeUnordered<CLIQUE>::jointBayesNet(Key j1, Key j2, const Eliminate& function) const
typename BayesTree<CLIQUE>::sharedBayesNet
BayesTree<CLIQUE>::jointBayesNet(Key j1, Key j2, const Eliminate& function) const
{
gttic(BayesTree_jointBayesNet);
// get clique C1 and C2
@ -385,7 +385,7 @@ namespace gtsam {
// Factor into C1\B | B.
sharedFactorGraph 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; {
std::vector<Index> C2_minus_B; {
@ -397,7 +397,7 @@ namespace gtsam {
// Factor into C2\B | B.
sharedFactorGraph 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);
@ -413,12 +413,12 @@ namespace gtsam {
p_BC1C2 += C2->conditional();
// 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>
void BayesTreeUnordered<CLIQUE>::clear() {
void BayesTree<CLIQUE>::clear() {
// Remove all nodes and clear the root pointer
nodes_.clear();
roots_.clear();
@ -426,7 +426,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::deleteCachedShortcuts() {
void BayesTree<CLIQUE>::deleteCachedShortcuts() {
BOOST_FOREACH(const sharedClique& root, roots_) {
root->deleteCachedShortcuts();
}
@ -434,7 +434,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::removeClique(sharedClique clique)
void BayesTree<CLIQUE>::removeClique(sharedClique clique)
{
if (clique->isRoot()) {
std::vector<sharedClique>::iterator root = std::find(roots_.begin(), roots_.end(), clique);
@ -458,7 +458,7 @@ namespace gtsam {
/* ************************************************************************* */
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
if (clique!=NULL) {
@ -483,7 +483,7 @@ namespace gtsam {
/* ************************************************************************* */
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
BOOST_FOREACH(const Key& j, keys)
@ -505,7 +505,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::Cliques BayesTreeUnordered<CLIQUE>::removeSubtree(
typename BayesTree<CLIQUE>::Cliques BayesTree<CLIQUE>::removeSubtree(
const sharedClique& subtree)
{
// Result clique list

677
gtsam/inference/BayesTree.h
View File

@ -1,418 +1,259 @@
/* ----------------------------------------------------------------------------
* 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/FactorGraph.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/linear/VectorValues.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 BayesTree {
public:
typedef BayesTree<CONDITIONAL, CLIQUE> This;
typedef boost::shared_ptr<BayesTree<CONDITIONAL, CLIQUE> > shared_ptr;
typedef boost::shared_ptr<CONDITIONAL> sharedConditional;
typedef boost::shared_ptr<BayesNet<CONDITIONAL> > sharedBayesNet;
typedef CONDITIONAL ConditionalType;
typedef typename CONDITIONAL::FactorType FactorType;
typedef typename FactorGraph<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 */
BayesTree() {}
/** Create a Bayes Tree from a Bayes Net (requires CONDITIONAL is IndexConditional *or* CONDITIONAL::Combine) */
explicit BayesTree(const BayesNet<CONDITIONAL>& bayesNet);
/** Copy constructor */
BayesTree(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.
*/
BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL,CLIQUE> > subtrees);
/** Destructor */
virtual ~BayesTree() {}
/// @}
/// @name Testable
/// @{
/** check equality */
bool equals(const BayesTree<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 BayesNet<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 FactorGraph<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 BayesNet<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, BayesNet<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, BayesNet<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(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>
/* ----------------------------------------------------------------------------
* 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 FactorGraph;
/**
* 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 BayesTree
{
protected:
typedef BayesTree<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 */
BayesTree() {}
/** Copy constructor */
BayesTree(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(FactorGraph<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

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

@ -16,7 +16,7 @@
#pragma once
#include <gtsam/inference/BayesTreeCliqueBaseUnordered.h>
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
@ -24,7 +24,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
bool BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::equals(
bool BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::equals(
const DERIVED& other, double tol = 1e-9) const
{
return (!conditional_ && !other.conditional())
@ -34,7 +34,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
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> indicesB(B->conditional()->begin(), B->conditional()->end());
@ -46,7 +46,7 @@ namespace gtsam {
/* ************************************************************************* */
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
{
gttic(shortcut_indices);
@ -65,7 +65,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
void BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::print(
void BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::print(
const std::string& s, const KeyFormatter& keyFormatter) const
{
conditional_->print(s, keyFormatter);
@ -73,7 +73,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
size_t BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::treeSize() const {
size_t BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::treeSize() const {
size_t size = 1;
BOOST_FOREACH(const derived_ptr& child, children)
size += child->treeSize();
@ -82,7 +82,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
size_t BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::numCachedSeparatorMarginals() const
size_t BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::numCachedSeparatorMarginals() const
{
if (!cachedSeparatorMarginal_)
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
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
typename BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::BayesNetType
BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::shortcut(const derived_ptr& B, Eliminate function) const
typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::BayesNetType
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
// and when the S\B is not empty
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
derived_ptr parent(parent_.lock());
gttoc(BayesTreeCliqueBaseUnordered_shortcut);
gttoc(BayesTreeCliqueBase_shortcut);
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)
// Determine the variables we want to keepSet, S union B
@ -121,7 +121,7 @@ namespace gtsam {
// Marginalize out everything except S union B
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
size_t nrFrontals = S_setminus_B.size();
@ -140,14 +140,14 @@ namespace gtsam {
// P(C) = P(F|S) P(S)
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
typename BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::FactorGraphType
BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::separatorMarginal(Eliminate function) const
typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separatorMarginal(Eliminate function) const
{
gttic(BayesTreeCliqueBaseUnordered_separatorMarginal);
gttic(BayesTreeCliqueBase_separatorMarginal);
// Check if the Separator marginal was already calculated
if (!cachedSeparatorMarginal_)
{
gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss);
gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
// If this is the root, there is no separator
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)
// initialize P(Cp) with the parent separator marginal
derived_ptr parent(parent_.lock());
gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); // Flatten recursion in timing outline
gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal);
gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss); // Flatten recursion in timing outline
gttoc(BayesTreeCliqueBase_separatorMarginal);
FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp)
gttic(BayesTreeCliqueBaseUnordered_separatorMarginal);
gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss);
gttic(BayesTreeCliqueBase_separatorMarginal);
gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
// now add the parent conditional
p_Cp += parent->conditional_; // P(Fp|Sp)
// The variables we want to keepSet are exactly the ones in S
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)
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
typename BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::FactorGraphType
BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::marginal2(Eliminate function) const
typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::marginal2(Eliminate function) const
{
gttic(BayesTreeCliqueBaseUnordered_marginal2);
gttic(BayesTreeCliqueBase_marginal2);
// initialize with separator marginal P(S)
FactorGraphType p_C = this->separatorMarginal(function);
// add the conditional P(F|S)
@ -196,7 +196,7 @@ namespace gtsam {
/* ************************************************************************* */
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
// 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,
* 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/FactorGraph.h>
#include <gtsam/inference/BayesNet.h>
namespace gtsam {
template<class CONDITIONAL, class CLIQUE> class BayesTree;
}
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 BayesTreeCliqueBase {
public:
typedef BayesTreeCliqueBase<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 FactorGraph<FactorType>::Eliminate Eliminate;
protected:
/// @name Standard Constructors
/// @{
/** Default constructor */
BayesTreeCliqueBase() {}
/** Construct from a conditional, leaving parent and child pointers uninitialized */
BayesTreeCliqueBase(const sharedConditional& conditional);
/** Construct from an elimination result, which is a pair<CONDITIONAL,FACTOR> */
BayesTreeCliqueBase(
const std::pair<sharedConditional,
boost::shared_ptr<typename ConditionalType::FactorType> >& result);
/// @}
/// This stores the Cached separator margnal P(S)
mutable boost::optional<FactorGraph<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 */
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);
}
}
/* ----------------------------------------------------------------------------
* 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 BayesTree;
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 BayesTreeCliqueBase
{
private:
typedef BayesTreeCliqueBase<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 */
BayesTreeCliqueBase() {}
/** Construct from a conditional, leaving parent and child pointers uninitialized */
BayesTreeCliqueBase(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 BayesTree<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_);
}
/// @}
};
}

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

@ -23,12 +23,12 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBase<DERIVED, CONDITIONAL>::assertInvariants() const {
void BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::assertInvariants() const {
}
/* ************************************************************************* */
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 {
sharedConditional p_F_S = this->conditional();
std::vector<Index> &indicesB = B->conditional()->keys();
@ -40,8 +40,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
std::vector<Index> BayesTreeCliqueBase<DERIVED, CONDITIONAL>::shortcut_indices(
derived_ptr B, const FactorGraph<FactorType>& p_Cp_B) const
std::vector<Index> BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::shortcut_indices(
derived_ptr B, const FactorGraphOrdered<FactorType>& p_Cp_B) const
{
gttic(shortcut_indices);
std::set<Index> allKeys = p_Cp_B.keys();
@ -60,7 +60,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
BayesTreeCliqueBase<DERIVED, CONDITIONAL>::BayesTreeCliqueBase(
BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::BayesTreeCliqueBaseOrdered(
const sharedConditional& conditional) :
conditional_(conditional) {
assertInvariants();
@ -68,7 +68,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
BayesTreeCliqueBase<DERIVED, CONDITIONAL>::BayesTreeCliqueBase(
BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::BayesTreeCliqueBaseOrdered(
const std::pair<sharedConditional,
boost::shared_ptr<typename ConditionalType::FactorType> >& result) :
conditional_(result.first) {
@ -77,14 +77,14 @@ namespace gtsam {
/* ************************************************************************* */
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 {
conditional_->print(s, indexFormatter);
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
size_t BayesTreeCliqueBase<DERIVED, CONDITIONAL>::treeSize() const {
size_t BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::treeSize() const {
size_t size = 1;
BOOST_FOREACH(const derived_ptr& child, children_)
size += child->treeSize();
@ -93,7 +93,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
size_t BayesTreeCliqueBase<DERIVED, CONDITIONAL>::numCachedSeparatorMarginals() const {
size_t BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::numCachedSeparatorMarginals() const {
if (!cachedSeparatorMarginal_)
return 0;
@ -106,7 +106,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBase<DERIVED, CONDITIONAL>::printTree(
void BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::printTree(
const std::string& indent, const IndexFormatter& indexFormatter) const {
asDerived(this)->print(indent, indexFormatter);
BOOST_FOREACH(const derived_ptr& child, children_)
@ -115,7 +115,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBase<DERIVED, CONDITIONAL>::permuteWithInverse(
void BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::permuteWithInverse(
const Permutation& inversePermutation) {
conditional_->permuteWithInverse(inversePermutation);
BOOST_FOREACH(const derived_ptr& child, children_) {
@ -126,7 +126,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
bool BayesTreeCliqueBase<DERIVED, CONDITIONAL>::reduceSeparatorWithInverse(
bool BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::reduceSeparatorWithInverse(
const internal::Reduction& 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
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
BayesNet<CONDITIONAL> BayesTreeCliqueBase<DERIVED, CONDITIONAL>::shortcut(
BayesNetOrdered<CONDITIONAL> BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::shortcut(
derived_ptr B, Eliminate function) const
{
gttic(BayesTreeCliqueBase_shortcut);
@ -163,7 +163,7 @@ namespace gtsam {
// Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
derived_ptr parent(parent_.lock());
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);
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
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
gttic(Undo_Reduce);
@ -197,7 +197,7 @@ namespace gtsam {
return result;
} else {
return BayesNet<CONDITIONAL>();
return BayesNetOrdered<CONDITIONAL>();
}
}
@ -206,7 +206,7 @@ namespace gtsam {
// P(C) = P(F|S) P(S)
/* ************************************************************************* */
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
{
gttic(BayesTreeCliqueBase_separatorMarginal);
@ -216,7 +216,7 @@ namespace gtsam {
// If this is the root, there is no separator
if (R.get() == this) {
// we are root, return empty
FactorGraph<FactorType> empty;
FactorGraphOrdered<FactorType> empty;
cachedSeparatorMarginal_ = empty;
} else {
// Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
@ -224,7 +224,7 @@ namespace gtsam {
derived_ptr parent(parent_.lock());
gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss); // Flatten recursion in timing outline
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_cachemiss);
// now add the parent conditional
@ -269,12 +269,12 @@ namespace gtsam {
// P(C) = P(F|S) P(S)
/* ************************************************************************* */
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
{
gttic(BayesTreeCliqueBase_marginal2);
// 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)
p_C.push_back(this->conditional()->toFactor());
return p_C;
@ -282,7 +282,7 @@ namespace gtsam {
/* ************************************************************************* */
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
// 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/FastSet.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/inference.h>
#include <gtsam/inference/BayesTreeOrdered.h>
#include <gtsam/inference/inferenceOrdered.h>
#include <gtsam/inference/GenericSequentialSolver.h>
#include <fstream>
@ -40,8 +40,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::CliqueData
BayesTree<CONDITIONAL,CLIQUE>::getCliqueData() const {
typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::CliqueData
BayesTreeOrdered<CONDITIONAL,CLIQUE>::getCliqueData() const {
CliqueData data;
getCliqueData(data, root_);
return data;
@ -49,7 +49,7 @@ namespace gtsam {
/* ************************************************************************* */
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.separatorSizes.push_back((*clique)->nrParents());
BOOST_FOREACH(sharedClique c, clique->children_) {
@ -59,13 +59,13 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
size_t BayesTree<CONDITIONAL,CLIQUE>::numCachedSeparatorMarginals() const {
size_t BayesTreeOrdered<CONDITIONAL,CLIQUE>::numCachedSeparatorMarginals() const {
return (root_) ? root_->numCachedSeparatorMarginals() : 0;
}
/* ************************************************************************* */
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!");
std::ofstream of(s.c_str());
of<< "digraph G{\n";
@ -76,7 +76,7 @@ namespace gtsam {
/* ************************************************************************* */
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;
bool first = true;
std::stringstream out;
@ -112,7 +112,7 @@ namespace gtsam {
/* ************************************************************************* */
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
<<"avg Conditional Size: " << avgConditionalSize << std::endl
<< "max Conditional Size: " << maxConditionalSize << std::endl
@ -122,8 +122,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::CliqueStats
BayesTree<CONDITIONAL,CLIQUE>::CliqueData::getStats() const {
typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::CliqueStats
BayesTreeOrdered<CONDITIONAL,CLIQUE>::CliqueData::getStats() const {
CliqueStats stats;
double sum = 0.0;
@ -149,7 +149,7 @@ namespace gtsam {
/* ************************************************************************* */
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";
BOOST_FOREACH(sharedClique clique, *this)
clique->printTree("", indexFormatter);
@ -157,14 +157,14 @@ namespace gtsam {
/* ************************************************************************* */
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;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique
BayesTree<CONDITIONAL,CLIQUE>::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) {
typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique
BayesTreeOrdered<CONDITIONAL,CLIQUE>::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) {
sharedClique new_clique(new Clique(conditional));
addClique(new_clique, parent_clique);
return new_clique;
@ -172,7 +172,7 @@ namespace gtsam {
/* ************************************************************************* */
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()));
BOOST_FOREACH(Index j, (*clique)->frontals())
nodes_[j] = clique;
@ -188,7 +188,7 @@ namespace gtsam {
/* ************************************************************************* */
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) {
sharedClique new_clique(new Clique(conditional));
nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size()));
@ -203,7 +203,7 @@ namespace gtsam {
/* ************************************************************************* */
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
if (root_)
root_->permuteWithInverse(inversePermutation);
@ -212,7 +212,7 @@ namespace gtsam {
Index maxIndex = *std::max_element(inversePermutation.begin(), inversePermutation.end());
// 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
for(size_t i = 0; i < nodes_.size(); ++i)
newNodes[inversePermutation[i]] = nodes_[i];
@ -222,7 +222,7 @@ namespace gtsam {
/* ************************************************************************* */
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;
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
// Debug check to make sure the conditional variable is ordered lower than
@ -249,7 +249,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::removeClique(sharedClique clique) {
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::removeClique(sharedClique clique) {
if (clique->isRoot())
root_.reset();
@ -271,9 +271,9 @@ namespace gtsam {
/* ************************************************************************* */
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 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
// symbolic tree, used in the BT(BN) constructor.
@ -282,21 +282,21 @@ namespace gtsam {
FastList<typename CONDITIONAL::shared_ptr> cliqueConditionals;
BOOST_FOREACH(Index j, symbolic->conditional()->frontals()) {
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
this->addClique(thisClique, parent);
// 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); }
}
/* ************************************************************************* */
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
BayesTree<IndexConditional> sbt(bayesNet);
BayesTreeOrdered<IndexConditionalOrdered> sbt(bayesNet);
// Build index of variables to conditionals
std::vector<boost::shared_ptr<CONDITIONAL> > conditionals(sbt.root()->conditional()->frontals().back() + 1);
@ -317,21 +317,21 @@ namespace gtsam {
/* ************************************************************************* */
template<>
inline BayesTree<IndexConditional>::BayesTree(const BayesNet<IndexConditional>& bayesNet) {
BayesNet<IndexConditional>::const_reverse_iterator rit;
inline BayesTreeOrdered<IndexConditionalOrdered>::BayesTreeOrdered(const BayesNetOrdered<IndexConditionalOrdered>& bayesNet) {
BayesNetOrdered<IndexConditionalOrdered>::const_reverse_iterator rit;
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
insert(*this, *rit);
}
/* ************************************************************************* */
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)
throw std::invalid_argument("BayesTree::insert: empty bayes net!");
// get the roots of child subtrees and merge their nodes_
std::list<sharedClique> childRoots;
typedef BayesTree<CONDITIONAL,CLIQUE> Tree;
typedef BayesTreeOrdered<CONDITIONAL,CLIQUE> Tree;
BOOST_FOREACH(const Tree& subtree, subtrees) {
nodes_.assign(subtree.nodes_.begin(), subtree.nodes_.end());
childRoots.push_back(subtree.root());
@ -339,7 +339,7 @@ namespace gtsam {
// create a new clique and add all the conditionals to the clique
sharedClique new_clique;
typename BayesNet<CONDITIONAL>::sharedConditional conditional;
typename BayesNetOrdered<CONDITIONAL>::sharedConditional conditional;
BOOST_REVERSE_FOREACH(conditional, bayesNet) {
if (!new_clique.get())
new_clique = addClique(conditional,childRoots);
@ -352,13 +352,13 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const This& other) {
BayesTreeOrdered<CONDITIONAL,CLIQUE>::BayesTreeOrdered(const This& other) {
*this = other;
}
/* ************************************************************************* */
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();
other.cloneTo(*this);
return *this;
@ -366,7 +366,7 @@ namespace gtsam {
/* ************************************************************************* */
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) {
std::cout << "WARNING: BayesTree.print encountered a forest..." << std::endl;
return;
@ -380,15 +380,15 @@ namespace gtsam {
// binary predicate to test equality of a pair for use in equals
template<class CONDITIONAL, class CLIQUE>
bool check_sharedCliques(
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& v1,
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& v2
const typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique& v1,
const typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique& v2
) {
return (!v1 && !v2) || (v1 && v2 && v1->equals(*v2));
}
/* ************************************************************************* */
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 {
return size()==other.size() &&
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 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());
assert(lowestOrderedParent != parents.end());
return *lowestOrderedParent;
@ -405,7 +405,7 @@ namespace gtsam {
/* ************************************************************************* */
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;
@ -442,7 +442,7 @@ namespace gtsam {
/* ************************************************************************* */
//TODO: remove this function after removing TSAM.cpp
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) {
if (clique->nrFrontals() == 0)
@ -457,18 +457,18 @@ namespace gtsam {
/* ************************************************************************* */
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
BOOST_FOREACH(const Index& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; }
// 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_) {
fillNodesIndex(child); }
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::insert(const sharedClique& subtree) {
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::insert(const sharedClique& subtree) {
if(subtree) {
// Find the parent clique of the new subtree. By the running intersection
// property, those separator variables in the subtree that are ordered
@ -497,7 +497,7 @@ namespace gtsam {
// First finds clique marginal then marginalizes that
/* ************************************************************************* */
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
{
gttic(BayesTree_marginalFactor);
@ -507,9 +507,9 @@ namespace gtsam {
// calculate or retrieve its marginal P(C) = P(F,S)
#ifdef OLD_SHORTCUT_MARGINALS
FactorGraph<FactorType> cliqueMarginal = clique->marginal(root_,function);
FactorGraphOrdered<FactorType> cliqueMarginal = clique->marginal(root_,function);
#else
FactorGraph<FactorType> cliqueMarginal = clique->marginal2(root_,function);
FactorGraphOrdered<FactorType> cliqueMarginal = clique->marginal2(root_,function);
#endif
// Reduce the variable indices to start at zero
@ -535,13 +535,13 @@ namespace gtsam {
/* ************************************************************************* */
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
{
gttic(BayesTree_marginalBayesNet);
// calculate marginal as a factor graph
FactorGraph<FactorType> fg;
FactorGraphOrdered<FactorType> fg;
fg.push_back(this->marginalFactor(j,function));
// Reduce the variable indices to start at zero
@ -553,7 +553,7 @@ namespace gtsam {
gttoc(Reduce);
// 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
gttic(Undo_Reduce);
@ -568,8 +568,8 @@ namespace gtsam {
// Find two cliques, their joint, then marginalizes
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename FactorGraph<typename CONDITIONAL::FactorType>::shared_ptr
BayesTree<CONDITIONAL,CLIQUE>::joint(Index j1, Index j2, Eliminate function) const {
typename FactorGraphOrdered<typename CONDITIONAL::FactorType>::shared_ptr
BayesTreeOrdered<CONDITIONAL,CLIQUE>::joint(Index j1, Index j2, Eliminate function) const {
gttic(BayesTree_joint);
// get clique C1 and C2
@ -605,13 +605,13 @@ namespace gtsam {
// Compute marginal on lowest common ancestor clique
gttic(LCA_marginal);
FactorGraph<FactorType> p_B = B->marginal2(this->root(), function);
FactorGraphOrdered<FactorType> p_B = B->marginal2(this->root(), function);
gttoc(LCA_marginal);
// Compute shortcuts of the requested cliques given the lowest common ancestor
gttic(Clique_shortcuts);
BayesNet<CONDITIONAL> p_C1_Bred = C1->shortcut(B, function);
BayesNet<CONDITIONAL> p_C2_Bred = C2->shortcut(B, function);
BayesNetOrdered<CONDITIONAL> p_C1_Bred = C1->shortcut(B, function);
BayesNetOrdered<CONDITIONAL> p_C2_Bred = C2->shortcut(B, function);
gttoc(Clique_shortcuts);
// 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());
}
// Factor into C1\B | B.
FactorGraph<FactorType> temp_remaining;
boost::tie(p_C1_B, temp_remaining) = FactorGraph<FactorType>(p_C1_Bred).eliminate(C1_minus_B, function);
FactorGraphOrdered<FactorType> temp_remaining;
boost::tie(p_C1_B, temp_remaining) = FactorGraphOrdered<FactorType>(p_C1_Bred).eliminate(C1_minus_B, function);
}
sharedConditional p_C2_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());
}
// Factor into C2\B | B.
FactorGraph<FactorType> temp_remaining;
boost::tie(p_C2_B, temp_remaining) = FactorGraph<FactorType>(p_C2_Bred).eliminate(C2_minus_B, function);
FactorGraphOrdered<FactorType> temp_remaining;
boost::tie(p_C2_B, temp_remaining) = FactorGraphOrdered<FactorType>(p_C2_Bred).eliminate(C2_minus_B, function);
}
gttoc(Full_root_factoring);
gttic(Variable_joint);
// 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_C1_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
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
gttic(Undo_Reduce);
@ -678,7 +678,7 @@ namespace gtsam {
/* ************************************************************************* */
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 {
// eliminate factor graph marginal to a Bayes net
@ -688,7 +688,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::clear() {
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::clear() {
// Remove all nodes and clear the root pointer
nodes_.clear();
root_.reset();
@ -696,8 +696,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::removePath(sharedClique clique,
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) {
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::removePath(sharedClique clique,
BayesNetOrdered<CONDITIONAL>& bn, typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::Cliques& orphans) {
// base case is NULL, if so we do nothing and return empties above
if (clique!=NULL) {
@ -722,8 +722,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
template<class CONTAINER>
void BayesTree<CONDITIONAL,CLIQUE>::removeTop(const CONTAINER& keys,
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) {
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::removeTop(const CONTAINER& keys,
BayesNetOrdered<CONDITIONAL>& bn, typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::Cliques& orphans) {
// process each key of the new factor
BOOST_FOREACH(const Index& j, keys) {
@ -746,7 +746,7 @@ namespace gtsam {
/* ************************************************************************* */
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)
{
// Result clique list
@ -783,14 +783,14 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::cloneTo(This& newTree) const {
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::cloneTo(This& newTree) const {
if(root())
cloneTo(newTree, root(), sharedClique());
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::cloneTo(
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::cloneTo(
This& newTree, const sharedClique& subtree, const sharedClique& parent) const {
sharedClique newClique(subtree->clone());
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 <boost/foreach.hpp>
#include <gtsam/inference/ClusterTree.h>
#include <gtsam/inference/ClusterTreeOrdered.h>
namespace gtsam {
@ -31,32 +31,32 @@ namespace gtsam {
* ************************************************************************* */
template<class FG>
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) {}
/* ************************************************************************* */
template<class FG>
template<typename FRONTALIT, typename SEPARATORIT>
ClusterTree<FG>::Cluster::Cluster(
ClusterTreeOrdered<FG>::Cluster::Cluster(
const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) :
FG(fg), frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {}
/* ************************************************************************* */
template<class FG>
template<typename FRONTALIT, typename SEPARATORIT>
ClusterTree<FG>::Cluster::Cluster(
ClusterTreeOrdered<FG>::Cluster::Cluster(
FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) :
frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {}
/* ************************************************************************* */
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);
}
/* ************************************************************************* */
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 (separator != other.separator) return false;
if (children_.size() != other.children_.size()) return false;
@ -71,7 +71,7 @@ namespace gtsam {
/* ************************************************************************* */
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 {
std::cout << indent;
BOOST_FOREACH(const Index key, frontal)
@ -84,7 +84,7 @@ namespace gtsam {
/* ************************************************************************* */
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 {
print(indent, formatter);
BOOST_FOREACH(const shared_ptr& child, children_)
@ -95,7 +95,7 @@ namespace gtsam {
* ClusterTree
* ************************************************************************* */
template<class FG>
void ClusterTree<FG>::print(const std::string& str,
void ClusterTreeOrdered<FG>::print(const std::string& str,
const IndexFormatter& formatter) const {
std::cout << str << std::endl;
if (root_) root_->printTree("", formatter);
@ -103,7 +103,7 @@ namespace gtsam {
/* ************************************************************************* */
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 false;
return root_->equals(*other.root_);

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

@ -38,7 +38,7 @@ namespace gtsam {
* \nosubgrouping
*/
template <class FG>
class ClusterTree {
class ClusterTreeOrdered {
public:
// Access to factor types
typedef typename FG::KeyType KeyType;
@ -113,7 +113,7 @@ namespace gtsam {
/// @{
/// constructor of empty tree
ClusterTree() {}
ClusterTreeOrdered() {}
/// @}
/// @name Standard Interface
@ -130,7 +130,7 @@ namespace gtsam {
void print(const std::string& str="", const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** 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
#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 <iostream>
#include <gtsam/inference/ConditionalUnordered.h>
#include <gtsam/inference/Conditional.h>
namespace gtsam {
/* ************************************************************************* */
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(";
BOOST_FOREACH(Key key, frontals())
std::cout << " " << formatter(key);
@ -40,7 +40,7 @@ namespace gtsam {
/* ************************************************************************* */
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_;
}

413
gtsam/inference/Conditional.h
View File

@ -1,266 +1,147 @@
/* ----------------------------------------------------------------------------
* 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/Factor.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 Conditional: public gtsam::Factor<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 {
Factor<KEY>::assertInvariants();
}
public:
typedef KEY KeyType;
typedef Conditional<KeyType> This;
typedef Factor<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::Factor<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 */
Conditional() :
nrFrontals_(0) {
assertInvariants();
}
/** No parents */
Conditional(KeyType key) :
FactorType(key), nrFrontals_(1) {
assertInvariants();
}
/** Single parent */
Conditional(KeyType key, KeyType parent) :
FactorType(key, parent), nrFrontals_(1) {
assertInvariants();
}
/** Two parents */
Conditional(KeyType key, KeyType parent1, KeyType parent2) :
FactorType(key, parent1, parent2), nrFrontals_(1) {
assertInvariants();
}
/** Three parents */
Conditional(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 */
Conditional(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 */
Conditional(const std::vector<Index>& keys, size_t nrFrontals) :
FactorType(keys), nrFrontals_(nrFrontals) {
assertInvariants();
}
/** 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();
}
/// @}
/// @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
/* ----------------------------------------------------------------------------
* 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 Conditional
{
protected:
/** The first nrFrontal variables are frontal and the rest are parents. */
size_t nrFrontals_;
private:
/// Typedef to this class
typedef Conditional<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 */
Conditional() : nrFrontals_(0) {}
/** Constructor */
Conditional(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

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
// for no variable index first so that it's always computed if we need to call COLAMD because
// no Ordering is provided.
return eliminateSequential(ordering, function, VariableIndexUnordered(asDerived()));
return eliminateSequential(ordering, function, VariableIndex(asDerived()));
}
else /*if(!ordering)*/ {
// 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'
// 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
// for no variable index first so that it's always computed if we need to call COLAMD because
// no Ordering is provided.
return eliminateMultifrontal(ordering, function, VariableIndexUnordered(asDerived()));
return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()));
}
else /*if(!ordering)*/ {
// 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'
// block.
return eliminateMultifrontal(OrderingUnordered::COLAMD(*variableIndex), function);
return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function);
}
}
@ -89,7 +89,7 @@ namespace gtsam {
template<class FACTORGRAPH>
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
const OrderingUnordered& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
{
if(variableIndex) {
gttic(eliminatePartialSequential);
@ -97,7 +97,7 @@ namespace gtsam {
return EliminationTreeType(asDerived(), *variableIndex, ordering).eliminate(function);
} else {
// 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>
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
const OrderingUnordered& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
{
if(variableIndex) {
gttic(eliminatePartialMultifrontal);
@ -113,7 +113,7 @@ namespace gtsam {
return JunctionTreeType(EliminationTreeType(asDerived(), *variableIndex, ordering)).eliminate(function);
} else {
// 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>
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
boost::variant<const OrderingUnordered&, const std::vector<Key>&> variables,
boost::variant<const Ordering&, const std::vector<Key>&> variables,
OptionalOrdering marginalizedVariableOrdering,
const Eliminate& function = EliminationTraits::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const
@ -136,7 +136,7 @@ namespace gtsam {
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > eliminated =
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
// 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
// COLAMD.
bool unmarginalizedAreOrdered = (boost::get<const OrderingUnordered&>(&variables) != 0);
bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
const std::vector<Key>* variablesOrOrdering =
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 =
OrderingUnordered::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
Ordering totalOrdering =
Ordering::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
// Split up ordering
const size_t nVars = variablesOrOrdering->size();
OrderingUnordered marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
OrderingUnordered marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
// Call this function again with the computed orderings
return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
}
} else {
// 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/variant.hpp>
#include <gtsam/inference/OrderingUnordered.h>
#include <gtsam/inference/VariableIndexUnordered.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/VariableIndex.h>
namespace gtsam {
@ -43,7 +43,7 @@ namespace gtsam {
// typedef MyJunctionTree JunctionTreeType; ///< Type of Junction tree (e.g. GaussianJunctionTree)
// static pair<shared_ptr<ConditionalType>, shared_ptr<FactorType>
// 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;
/// 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 boost::optional<const OrderingUnordered&> OptionalOrdering;
typedef boost::optional<const Ordering&> OptionalOrdering;
/// 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
* provided, the ordering provided by COLAMD will be used.
@ -149,7 +149,7 @@ namespace gtsam {
* B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialSequential(
const OrderingUnordered& ordering,
const Ordering& ordering,
const Eliminate& function = EliminationTraits::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
@ -169,7 +169,7 @@ namespace gtsam {
* factor graph, and \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialMultifrontal(
const OrderingUnordered& ordering,
const Ordering& ordering,
const Eliminate& function = EliminationTraits::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
@ -195,7 +195,7 @@ namespace gtsam {
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
* provided one will be computed. */
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,
const Eliminate& function = EliminationTraits::DefaultEliminate,
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/treeTraversal-inst.h>
#include <gtsam/inference/EliminationTreeUnordered.h>
#include <gtsam/inference/VariableIndexUnordered.h>
#include <gtsam/inference/OrderingUnordered.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/inference-inst.h>
namespace gtsam {
/* ************************************************************************* */
template<class BAYESNET, class GRAPH>
typename EliminationTreeUnordered<BAYESNET,GRAPH>::sharedFactor
EliminationTreeUnordered<BAYESNET,GRAPH>::Node::eliminate(
typename EliminationTree<BAYESNET,GRAPH>::sharedFactor
EliminationTree<BAYESNET,GRAPH>::Node::eliminate(
const boost::shared_ptr<BayesNetType>& output,
const Eliminate& function, const std::vector<sharedFactor>& childrenResults) const
{
@ -51,7 +51,7 @@ namespace gtsam {
// Do dense elimination step
std::vector<Key> keyAsVector(1); keyAsVector[0] = key;
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
function(gatheredFactors, OrderingUnordered(keyAsVector));
function(gatheredFactors, Ordering(keyAsVector));
// Add conditional to BayesNet
output->push_back(eliminationResult.first);
@ -62,7 +62,7 @@ namespace gtsam {
/* ************************************************************************* */
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
{
std::cout << str << "(" << keyFormatter(key) << ")\n";
@ -77,8 +77,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class BAYESNET, class GRAPH>
EliminationTreeUnordered<BAYESNET,GRAPH>::EliminationTreeUnordered(const FactorGraphType& graph,
const VariableIndexUnordered& structure, const OrderingUnordered& order)
EliminationTree<BAYESNET,GRAPH>::EliminationTree(const FactorGraphType& graph,
const VariableIndex& structure, const Ordering& order)
{
gttic(EliminationTree_Contructor);
@ -100,7 +100,7 @@ namespace gtsam {
for (size_t j = 0; j < n; j++)
{
// 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]->key = order[j];
@ -157,20 +157,20 @@ namespace gtsam {
/* ************************************************************************* */
template<class BAYESNET, class GRAPH>
EliminationTreeUnordered<BAYESNET,GRAPH>::EliminationTreeUnordered(
const FactorGraphType& factorGraph, const OrderingUnordered& order)
EliminationTree<BAYESNET,GRAPH>::EliminationTree(
const FactorGraphType& factorGraph, const Ordering& order)
{
gttic(ET_Create2);
// Build variable index first
const VariableIndexUnordered variableIndex(factorGraph);
const VariableIndex variableIndex(factorGraph);
This temp(factorGraph, variableIndex, order);
this->swap(temp); // Swap in the tree, and temp will be deleted
}
/* ************************************************************************* */
template<class BAYESNET, class GRAPH>
EliminationTreeUnordered<BAYESNET,GRAPH>&
EliminationTreeUnordered<BAYESNET,GRAPH>::operator=(const EliminationTreeUnordered<BAYESNET,GRAPH>& other)
EliminationTree<BAYESNET,GRAPH>&
EliminationTree<BAYESNET,GRAPH>::operator=(const EliminationTree<BAYESNET,GRAPH>& other)
{
// Start by duplicating the tree.
roots_ = treeTraversal::CloneForest(other);
@ -185,7 +185,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class BAYESNET, class 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);
// Allocate result
@ -205,14 +205,14 @@ namespace gtsam {
/* ************************************************************************* */
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);
}
/* ************************************************************************* */
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
std::stack<sharedNode, std::vector<sharedNode> > stack1, stack2;
@ -281,7 +281,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class BAYESNET, class GRAPH>
void EliminationTreeUnordered<BAYESNET,GRAPH>::swap(This& other) {
void EliminationTree<BAYESNET,GRAPH>::swap(This& other) {
roots_.swap(other.roots_);
remainingFactors_.swap(other.remainingFactors_);
}

354
gtsam/inference/EliminationTree.h
View File

@ -1,187 +1,167 @@
/* ----------------------------------------------------------------------------
* 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/VariableIndex.h>
#include <gtsam/inference/FactorGraph.h>
class EliminationTreeTester; // for unit tests, see testEliminationTree
namespace gtsam { template<class CONDITIONAL> class BayesNet; }
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 EliminationTree {
public:
typedef EliminationTree<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::BayesNet<typename FACTOR::ConditionalType> BayesNet; ///< The BayesNet corresponding to FACTOR
typedef FACTOR Factor;
typedef typename FACTOR::KeyType KeyType;
/** Typedef for an eliminate subroutine */
typedef typename FactorGraph<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 FactorGraph<DERIVEDFACTOR>& factorGraph, const VariableIndex& 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 FactorGraph<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 EliminationTree& other, double tol = 1e-9) const;
/// @}
private:
/** default constructor, private, as you should use Create below */
EliminationTree(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 VariableIndex& 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 ::EliminationTreeTester;
};
/**
* 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/EliminationTree-inl.h>
/* ----------------------------------------------------------------------------
* 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 EliminationTreeTester; // for unit tests, see testEliminationTree
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
* 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 EliminationTree
{
protected:
typedef EliminationTree<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
*/
EliminationTree(const FactorGraphType& factorGraph,
const VariableIndex& structure, const Ordering& 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
*/
EliminationTree(const FactorGraphType& factorGraph, const Ordering& order);
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
* copied, factors are not cloned. */
EliminationTree(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
EliminationTree() {}
private:
/// Allow access to constructor and add methods for testing purposes
friend class ::EliminationTreeTester;
};
}

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/FastSet.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/inference/EliminationTreeOrdered.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#include <boost/foreach.hpp>
#include <boost/static_assert.hpp>
@ -34,7 +34,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminate_(
typename EliminationTreeOrdered<FACTOR>::sharedFactor EliminationTreeOrdered<FACTOR>::eliminate_(
Eliminate function, Conditionals& conditionals) const {
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
// 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());
// 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
// 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));
conditionals[this->key_] = eliminated.first;
@ -75,7 +75,7 @@ typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminat
/* ************************************************************************* */
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
const size_t m = structure.nFactors();
@ -109,9 +109,9 @@ std::vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex&
/* ************************************************************************* */
template<class FACTOR>
template<class DERIVEDFACTOR>
typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
const FactorGraph<DERIVEDFACTOR>& factorGraph,
const VariableIndex& structure) {
typename EliminationTreeOrdered<FACTOR>::shared_ptr EliminationTreeOrdered<FACTOR>::Create(
const FactorGraphOrdered<DERIVEDFACTOR>& factorGraph,
const VariableIndexOrdered& structure) {
static const bool debug = false;
gttic(ET_Create1);
@ -131,7 +131,7 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
std::vector<shared_ptr> trees(n);
for (Index k = 1; k <= n; k++) {
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
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
@ -161,12 +161,12 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
/* ************************************************************************* */
template<class FACTOR>
template<class DERIVEDFACTOR>
typename EliminationTree<FACTOR>::shared_ptr
EliminationTree<FACTOR>::Create(const FactorGraph<DERIVEDFACTOR>& factorGraph) {
typename EliminationTreeOrdered<FACTOR>::shared_ptr
EliminationTreeOrdered<FACTOR>::Create(const FactorGraphOrdered<DERIVEDFACTOR>& factorGraph) {
gttic(ET_Create2);
// Build variable index
const VariableIndex variableIndex(factorGraph);
const VariableIndexOrdered variableIndex(factorGraph);
// Build elimination tree
return Create(factorGraph, variableIndex);
@ -174,7 +174,7 @@ EliminationTree<FACTOR>::Create(const FactorGraph<DERIVEDFACTOR>& factorGraph) {
/* ************************************************************************* */
template<class FACTORGRAPH>
void EliminationTree<FACTORGRAPH>::print(const std::string& name,
void EliminationTreeOrdered<FACTORGRAPH>::print(const std::string& name,
const IndexFormatter& formatter) const {
std::cout << name << " (" << formatter(key_) << ")" << std::endl;
BOOST_FOREACH(const sharedFactor& factor, factors_) {
@ -185,7 +185,7 @@ void EliminationTree<FACTORGRAPH>::print(const std::string& name,
/* ************************************************************************* */
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_
&& this->subTrees_.size() == expected.subTrees_.size()) {
typename SubTrees::const_iterator this_subtree = this->subTrees_.begin();
@ -200,8 +200,8 @@ bool EliminationTree<FACTORGRAPH>::equals(const EliminationTree<FACTORGRAPH>& ex
/* ************************************************************************* */
template<class FACTOR>
typename EliminationTree<FACTOR>::BayesNet::shared_ptr
EliminationTree<FACTOR>::eliminatePartial(typename EliminationTree<FACTOR>::Eliminate function, size_t nrToEliminate) const {
typename EliminationTreeOrdered<FACTOR>::BayesNet::shared_ptr
EliminationTreeOrdered<FACTOR>::eliminatePartial(typename EliminationTreeOrdered<FACTOR>::Eliminate function, size_t nrToEliminate) const {
// call recursive routine
gttic(ET_recursive_eliminate);
@ -225,8 +225,8 @@ typename EliminationTree<FACTOR>::BayesNet::shared_ptr
/* ************************************************************************* */
template<class FACTOR>
typename EliminationTree<FACTOR>::BayesNet::shared_ptr
EliminationTree<FACTOR>::eliminate(Eliminate function) const {
typename EliminationTreeOrdered<FACTOR>::BayesNet::shared_ptr
EliminationTreeOrdered<FACTOR>::eliminate(Eliminate function) const {
size_t nrConditionals = this->key_ + 1; // root key has highest index
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 <iostream>
#include <gtsam/inference/FactorUnordered.h>
#include <gtsam/inference/Factor.h>
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);
}
/* ************************************************************************* */
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 << " ";
BOOST_FOREACH(Key key, keys_) std::cout << " " << formatter(key);
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_;
}

271
gtsam/inference/Factor.h
View File

@ -21,212 +21,151 @@
#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>
#include <gtsam/base/types.h>
#include <gtsam/inference/Key.h>
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,
* derived classes must redefine this.
* 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
*/
typedef Conditional<KeyType> ConditionalType;
class GTSAM_EXPORT Factor
{
/// A shared_ptr to this class, derived classes must redefine this.
typedef boost::shared_ptr<Factor> shared_ptr;
private:
// 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
typedef typename std::vector<KeyType>::iterator iterator;
public:
/// Iterator over keys
typedef std::vector<Key>::iterator iterator;
/// Const iterator over keys
typedef typename std::vector<KeyType>::const_iterator const_iterator;
/// Const iterator over keys
typedef std::vector<Key>::const_iterator const_iterator;
protected:
protected:
/// The keys involved in this factor
std::vector<KeyType> keys_;
/// The keys involved in this factor
std::vector<Key> keys_;
public:
/// @name Standard Constructors
/// @{
/// @name Standard Constructors
/// @{
/** Default constructor for I/O */
Factor() {}
/** Copy constructor */
Factor(const This& f);
/** 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>
Factor(const CONTAINER& keys) : keys_(keys.begin(), keys.end()) {}
/** Construct from conditional, calls ConditionalType::toFactor() */
Factor(const ConditionalType& c);
/** 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>
Factor(ITERATOR first, ITERATOR last) : keys_(first, last) {}
/** Default constructor for I/O */
Factor() {}
/** 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 Factor FromKeys(const CONTAINER& keys) {
return Factor(keys.begin(), keys.end()); }
/** Construct unary factor */
Factor(KeyType key) : keys_(1) {
keys_[0] = key; assertInvariants(); }
/** 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 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 */
Factor(KeyType key1, KeyType key2, KeyType key3) : keys_(3) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); }
public:
/// @name Standard Interface
/// @{
/** Construct 4-way factor */
Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4) : keys_(4) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); }
/// First key
Key front() const { return keys_.front(); }
/** Construct 5-way factor */
Factor(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(); }
/// Last key
Key back() const { return keys_.back(); }
/** Construct 6-way factor */
Factor(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(); }
/// find
const_iterator find(Key key) const { return std::find(begin(), end(), key); }
/// @}
/// @name Advanced Constructors
/// @{
/// Access the factor's involved variable keys
const std::vector<Key>& keys() const { return keys_; }
/** Construct n-way factor */
Factor(const std::set<KeyType>& keys) {
BOOST_FOREACH(const KeyType& key, keys) keys_.push_back(key);
assertInvariants();
}
/** Iterator at beginning of involved variable keys */
const_iterator begin() const { return keys_.begin(); }
/** Construct n-way factor */
Factor(const std::vector<KeyType>& keys) : keys_(keys) {
assertInvariants();
}
/** Iterator at end of involved variable keys */
const_iterator end() const { return keys_.end(); }
/** Constructor from a collection of keys */
template<class KEYITERATOR> Factor(KEYITERATOR beginKey, KEYITERATOR endKey) :
keys_(beginKey, endKey) { assertInvariants(); }
/**
* @return the number of variables involved in this factor
*/
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();
/**
* eliminate the first nrFrontals frontal variables.
*/
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::shared_ptr eliminate(size_t nrFrontals = 1);
#endif
/// @name Testable
/// @{
/// @name Standard Interface
/// @{
/// print
void print(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// First key
KeyType front() const { return keys_.front(); }
/// print only keys
void printKeys(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// Last key
KeyType back() const { return keys_.back(); }
protected:
/// 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
const std::vector<KeyType>& keys() const { return keys_; }
public:
/// @name Advanced Interface
/// @{
/** iterators */
const_iterator begin() const { return keys_.begin(); } ///TODO: comment
const_iterator end() const { return keys_.end(); } ///TODO: comment
/** @return keys involved in this factor */
std::vector<Key>& keys() { return keys_; }
/**
* @return the number of variables involved in this factor
*/
size_t size() const { return keys_.size(); }
/** Iterator at beginning of involved variable keys */
iterator begin() { return keys_.begin(); }
/// @}
/// @name Testable
/// @{
/** Iterator at end of involved variable keys */
iterator end() { return keys_.end(); }
/// print
void print(const std::string& s = "Factor",
const IndexFormatter& formatter = DefaultIndexFormatter) 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_);
}
/// 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/treeTraversal-inst.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/FactorGraphUnordered.h>
#include <gtsam/inference/BayesTreeUnordered.h>
#include <gtsam/inference/VariableIndexOrdered.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/BayesTree.h>
#include <boost/foreach.hpp>
#include <boost/bind.hpp>
@ -37,7 +37,7 @@ namespace gtsam {
/* ************************************************************************* */
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 << "size: " << size() << std::endl;
for (size_t i = 0; i < factors_.size(); i++) {
@ -50,7 +50,7 @@ namespace gtsam {
/* ************************************************************************* */
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_ */
if (factors_.size() != fg.size()) return false;
@ -67,7 +67,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
size_t FactorGraphUnordered<FACTOR>::nrFactors() const {
size_t FactorGraph<FACTOR>::nrFactors() const {
size_t size_ = 0;
BOOST_FOREACH(const sharedFactor& factor, factors_)
if (factor) size_++;
@ -76,7 +76,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
FastSet<Key> FactorGraphUnordered<FACTOR>::keys() const {
FastSet<Key> FactorGraph<FACTOR>::keys() const {
FastSet<Key> allKeys;
BOOST_FOREACH(const sharedFactor& factor, factors_)
if (factor)

337
gtsam/inference/FactorGraph.h
View File

@ -15,25 +15,59 @@
* @author Carlos Nieto
* @author Christian Potthast
* @author Michael Kaess
* @author Richard Roberts
*/
// \callgraph
#pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/BayesNet.h>
#include <boost/serialization/nvp.hpp>
#include <boost/function.hpp>
#include <set>
#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 CONDITIONAL, class CLIQUE> class BayesTree;
class VariableIndex;
// Forward declarations
template<class CLIQUE> class BayesTree;
/** 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.
@ -44,62 +78,64 @@ class VariableIndex;
class FactorGraph {
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 FactorGraph<FACTOR> This; ///< Typedef for this class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer for this class
typedef sharedFactor value_type;
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;
private:
typedef FactorGraph<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_;
public:
/// @name Standard Constructor
/// @name Standard Constructors
/// @{
/** Default constructor */
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
/// @{
// 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);
///**
// * @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 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());
}
///** convert from a derived type */
//template<class DERIVEDFACTOR>
//FactorGraph(const FactorGraph<DERIVEDFACTOR>& factors) {
// factors_.assign(factors.begin(), factors.end());
//}
/// @}
public:
/// @name Adding Factors
/// @{
@ -109,69 +145,159 @@ class VariableIndex;
*/
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>
void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) {
factors_.push_back(boost::shared_ptr<FACTOR>(factor));
}
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)); }
/** push back many factors */
void push_back(const This& factors) {
factors_.insert(end(), factors.begin(), factors.end());
}
/** 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 */
/** push back many factors with an iterator over shared_ptr (factors are not copied) */
template<typename ITERATOR>
void push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
factors_.insert(end(), firstFactor, lastFactor);
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());
}
/**
* @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());
/** 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 BayesTree<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 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
/// @{
/** print out graph */
void print(const std::string& s = "FactorGraph",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
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 and NULLS */
size_t size() const { return factors_.size();}
/** return the number of factors (including any null factors set by remove() ). */
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(); }
/** const cast to the underlying vector of factors */
operator const std::vector<sharedFactor>&() const { return factors_; }
/** 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 */
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]; }
/** 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); }
/** 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 */
/** Iterator to beginning of factors. */
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(); }
/** Get the first factor */
@ -180,38 +306,6 @@ class VariableIndex;
/** 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, 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)
/// @{
@ -222,24 +316,33 @@ class VariableIndex;
/** non-const STL-style 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); }
/** 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 */
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
/// @{
/** return the number valid factors */
/** return the number of non-null factors */
size_t nrFactors() const;
/** Potentially very slow function to return all keys involved */
std::set<KeyType> keys() const;
FastSet<Key> keys() const;
private:
@ -254,20 +357,4 @@ class VariableIndex;
}; // 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
#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
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/BayesTreeOrdered.h>
#include <gtsam/inference/VariableIndexOrdered.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
@ -41,7 +41,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
template<class CONDITIONAL>
FactorGraph<FACTOR>::FactorGraph(const BayesNet<CONDITIONAL>& bayesNet) {
FactorGraphOrdered<FACTOR>::FactorGraphOrdered(const BayesNetOrdered<CONDITIONAL>& bayesNet) {
factors_.reserve(bayesNet.size());
BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, bayesNet) {
this->push_back(cond->toFactor());
@ -50,7 +50,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
void FactorGraph<FACTOR>::print(const std::string& s,
void FactorGraphOrdered<FACTOR>::print(const std::string& s,
const IndexFormatter& formatter) const {
std::cout << s << std::endl;
std::cout << "size: " << size() << std::endl;
@ -63,7 +63,7 @@ namespace gtsam {
/* ************************************************************************* */
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_ */
if (factors_.size() != fg.size()) return false;
@ -80,7 +80,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
size_t FactorGraph<FACTOR>::nrFactors() const {
size_t FactorGraphOrdered<FACTOR>::nrFactors() const {
size_t size_ = 0;
BOOST_FOREACH(const sharedFactor& factor, factors_)
if (factor) size_++;
@ -89,7 +89,7 @@ namespace gtsam {
/* ************************************************************************* */
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;
BOOST_FOREACH(const sharedFactor& factor, factors_)
if (factor) {
@ -101,11 +101,11 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
std::pair<typename FactorGraph<FACTOR>::sharedConditional, FactorGraph<FACTOR> >
FactorGraph<FACTOR>::eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const
std::pair<typename FactorGraphOrdered<FACTOR>::sharedConditional, FactorGraphOrdered<FACTOR> >
FactorGraphOrdered<FACTOR>::eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const
{
// Build variable index
VariableIndex variableIndex(*this);
VariableIndexOrdered variableIndex(*this);
// Find first variable
Index firstIndex = 0;
@ -125,8 +125,8 @@ namespace gtsam {
}
// Separate factors into involved and remaining
FactorGraph<FactorType> involvedFactors;
FactorGraph<FactorType> remainingFactors;
FactorGraphOrdered<FactorType> involvedFactors;
FactorGraphOrdered<FactorType> remainingFactors;
FastSet<size_t>::const_iterator involvedFactorIsIt = involvedFactorIs.begin();
for(size_t i = 0; i < this->size(); ++i) {
if(involvedFactorIsIt != involvedFactorIs.end() && *involvedFactorIsIt == i) {
@ -140,7 +140,7 @@ namespace gtsam {
}
// Do dense elimination on the involved factors
typename FactorGraph<FactorType>::EliminationResult eliminationResult =
typename FactorGraphOrdered<FactorType>::EliminationResult eliminationResult =
eliminate(involvedFactors, nFrontals);
// Add the remaining factor back into the factor graph
@ -152,15 +152,15 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
std::pair<typename FactorGraph<FACTOR>::sharedConditional, FactorGraph<FACTOR> >
FactorGraph<FACTOR>::eliminate(const std::vector<KeyType>& variables, const Eliminate& eliminateFcn,
boost::optional<const VariableIndex&> variableIndex_) const
std::pair<typename FactorGraphOrdered<FACTOR>::sharedConditional, FactorGraphOrdered<FACTOR> >
FactorGraphOrdered<FACTOR>::eliminate(const std::vector<KeyType>& variables, const Eliminate& eliminateFcn,
boost::optional<const VariableIndexOrdered&> variableIndex_) const
{
const VariableIndex& variableIndex =
variableIndex_ ? *variableIndex_ : VariableIndex(*this);
const VariableIndexOrdered& variableIndex =
variableIndex_ ? *variableIndex_ : VariableIndexOrdered(*this);
// First find the involved factors
FactorGraph<FACTOR> involvedFactors;
FactorGraphOrdered<FACTOR> involvedFactors;
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
@ -205,7 +205,7 @@ namespace gtsam {
}
// Build the remaining graph, without the removed factors
FactorGraph<FACTOR> remainingGraph;
FactorGraphOrdered<FACTOR> remainingGraph;
remainingGraph.reserve(this->size() - involvedFactors.size() + 1);
FastSet<size_t>::const_iterator involvedFactorIndexIt = involvedFactorIndices.begin();
for(size_t i = 0; i < this->size(); ++i) {
@ -224,7 +224,7 @@ namespace gtsam {
}
/* ************************************************************************* */
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(
boost::format("Factor graph does not contain a factor with index %d.")
% index));
@ -246,7 +246,7 @@ namespace gtsam {
/* ************************************************************************* */
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) {
typedef const std::pair<const KEY, std::vector<KEY> > KeySlotPair;
@ -263,14 +263,14 @@ namespace gtsam {
template<class FACTOR, class CONDITIONAL, class CLIQUE>
void _FactorGraph_BayesTree_adder(
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) {
// Add factor from this clique
factors_io.push_back((*clique)->toFactor());
// Traverse children
typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
typedef typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& child, clique->children())
_FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors_io, child);
}
@ -279,7 +279,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
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());
_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
#include <gtsam/inference/Factor.h>
#include <gtsam/inference/FactorOrdered.h>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <iostream>
@ -27,14 +27,14 @@ namespace gtsam {
/* ************************************************************************* */
template<typename KEY>
Factor<KEY>::Factor(const Factor<KEY>& f) :
FactorOrdered<KEY>::FactorOrdered(const FactorOrdered<KEY>& f) :
keys_(f.keys_) {
assertInvariants();
}
/* ************************************************************************* */
template<typename KEY>
Factor<KEY>::Factor(const ConditionalType& c) :
FactorOrdered<KEY>::FactorOrdered(const ConditionalType& c) :
keys_(c.keys()) {
// assert(c.nrFrontals() == 1);
assertInvariants();
@ -42,7 +42,7 @@ namespace gtsam {
/* ************************************************************************* */
template<typename KEY>
void Factor<KEY>::assertInvariants() const {
void FactorOrdered<KEY>::assertInvariants() const {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
// Check that keys are all unique
std::multiset<KeyType> nonunique(keys_.begin(), keys_.end());
@ -56,14 +56,14 @@ namespace gtsam {
/* ************************************************************************* */
template<typename KEY>
void Factor<KEY>::print(const std::string& s,
void FactorOrdered<KEY>::print(const std::string& s,
const IndexFormatter& formatter) const {
printKeys(s,formatter);
}
/* ************************************************************************* */
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 << " ";
BOOST_FOREACH(KEY key, keys_) std::cout << " " << formatter(key);
std::cout << std::endl;
@ -71,7 +71,7 @@ namespace gtsam {
/* ************************************************************************* */
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_;
}
@ -79,7 +79,7 @@ namespace gtsam {
#ifdef TRACK_ELIMINATE
template<typename KEY>
template<class COND>
typename COND::shared_ptr Factor<KEY>::eliminateFirst() {
typename COND::shared_ptr FactorOrdered<KEY>::eliminateFirst() {
assert(!keys_.empty());
assertInvariants();
KEY eliminated = keys_.front();
@ -91,10 +91,10 @@ namespace gtsam {
/* ************************************************************************* */
template<typename KEY>
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);
assertInvariants();
typename BayesNet<COND>::shared_ptr fragment(new BayesNet<COND> ());
typename BayesNetOrdered<COND>::shared_ptr fragment(new BayesNetOrdered<COND> ());
const_iterator it = this->begin();
for (KEY n = 0; n < nrFrontals; ++n, ++it)
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
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/FactorOrdered-inl.h>
#include <gtsam/inference/JunctionTreeOrdered.h>
#include <gtsam/inference/BayesNetOrdered-inl.h>
namespace gtsam {
/* ************************************************************************* */
template<class F, class JT>
GenericMultifrontalSolver<F, JT>::GenericMultifrontalSolver(
const FactorGraph<F>& graph) :
structure_(new VariableIndex(graph)), junctionTree_(
const FactorGraphOrdered<F>& graph) :
structure_(new VariableIndexOrdered(graph)), junctionTree_(
new JT(graph, *structure_)) {
}
@ -35,7 +35,7 @@ namespace gtsam {
template<class F, class JT>
GenericMultifrontalSolver<F, JT>::GenericMultifrontalSolver(
const sharedGraph& graph,
const VariableIndex::shared_ptr& variableIndex) :
const VariableIndexOrdered::shared_ptr& variableIndex) :
structure_(variableIndex), junctionTree_(new JT(*graph, *structure_)) {
}
@ -63,16 +63,16 @@ namespace gtsam {
/* ************************************************************************* */
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 {
// eliminate junction tree, returns pointer to root
typename BayesTree<typename FACTOR::ConditionalType>::sharedClique
typename BayesTreeOrdered<typename FACTOR::ConditionalType>::sharedClique
root = junctionTree_->eliminate(function);
// create an empty Bayes tree and insert root clique
typename BayesTree<typename FACTOR::ConditionalType>::shared_ptr
bayesTree(new BayesTree<typename FACTOR::ConditionalType>);
typename BayesTreeOrdered<typename FACTOR::ConditionalType>::shared_ptr
bayesTree(new BayesTreeOrdered<typename FACTOR::ConditionalType>);
bayesTree->insert(root);
// return the Bayes tree
@ -81,7 +81,7 @@ namespace gtsam {
/* ************************************************************************* */
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 {
// FIXME: joint for arbitrary sets of variables not present

18
gtsam/inference/GenericMultifrontalSolver.h
View File

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

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

@ -18,11 +18,11 @@
#pragma once
#include <gtsam/inference/Factor.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/inference.h>
#include <gtsam/inference/FactorOrdered.h>
#include <gtsam/inference/FactorGraphOrdered.h>
#include <gtsam/inference/EliminationTreeOrdered.h>
#include <gtsam/inference/BayesNetOrdered.h>
#include <gtsam/inference/inferenceOrdered.h>
#include <boost/foreach.hpp>
@ -30,24 +30,24 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph) {
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const FactorGraphOrdered<FACTOR>& factorGraph) {
gttic(GenericSequentialSolver_constructor1);
assert(factorGraph.size());
factors_.reset(new FactorGraph<FACTOR>(factorGraph));
structure_.reset(new VariableIndex(factorGraph));
eliminationTree_ = EliminationTree<FACTOR>::Create(*factors_, *structure_);
factors_.reset(new FactorGraphOrdered<FACTOR>(factorGraph));
structure_.reset(new VariableIndexOrdered(factorGraph));
eliminationTree_ = EliminationTreeOrdered<FACTOR>::Create(*factors_, *structure_);
}
/* ************************************************************************* */
template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(
const sharedFactorGraph& factorGraph,
const boost::shared_ptr<VariableIndex>& variableIndex)
const boost::shared_ptr<VariableIndexOrdered>& variableIndex)
{
gttic(GenericSequentialSolver_constructor2);
factors_ = factorGraph;
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.
eliminationTree_.reset();
factors_ = factorGraph;
eliminationTree_ = EliminationTree<FACTOR>::Create(*factors_, *structure_);
eliminationTree_ = EliminationTreeOrdered<FACTOR>::Create(*factors_, *structure_);
}
/* ************************************************************************* */
@ -107,7 +107,7 @@ namespace gtsam {
factor->permuteWithInverse(*permutationInverse);
// 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;
if(nrToEliminate)
bayesNet = etree->eliminatePartial(function, *nrToEliminate);
@ -175,15 +175,15 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
typename FactorGraph<FACTOR>::shared_ptr //
typename FactorGraphOrdered<FACTOR>::shared_ptr //
GenericSequentialSolver<FACTOR>::jointFactorGraph(
const std::vector<Index>& js, Eliminate function) const
{
gttic(GenericSequentialSolver_jointFactorGraph);
// 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>
namespace gtsam {
class VariableIndex;
class VariableIndexOrdered;
class Permutation;
}
namespace gtsam {
template<class FACTOR> class EliminationTree;
template<class FACTOR> class EliminationTreeOrdered;
}
namespace gtsam {
template<class FACTOR> class FactorGraph;
template<class FACTOR> class FactorGraphOrdered;
}
namespace gtsam {
template<class CONDITIONAL> class BayesNet;
template<class CONDITIONAL> class BayesNetOrdered;
}
namespace gtsam {
@ -62,13 +62,13 @@ namespace gtsam {
protected:
typedef boost::shared_ptr<FactorGraph<FACTOR> > sharedFactorGraph;
typedef boost::shared_ptr<FactorGraphOrdered<FACTOR> > sharedFactorGraph;
typedef typename FACTOR::ConditionalType Conditional;
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 boost::function<
EliminationResult(const FactorGraph<FACTOR>&, size_t)> Eliminate;
EliminationResult(const FactorGraphOrdered<FACTOR>&, size_t)> Eliminate;
/** Store the original factors for computing marginals
* TODO Frank says: really? Marginals should be computed from result.
@ -76,14 +76,14 @@ namespace gtsam {
sharedFactorGraph factors_;
/** Store column structure of the factor graph. Why? */
boost::shared_ptr<VariableIndex> structure_;
boost::shared_ptr<VariableIndexOrdered> structure_;
/** Elimination tree that performs elimination */
boost::shared_ptr<EliminationTree<FACTOR> > eliminationTree_;
boost::shared_ptr<EliminationTreeOrdered<FACTOR> > eliminationTree_;
/** concept checks */
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
// GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree)
// GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTreeOrdered)
/**
* 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
* 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
@ -109,7 +109,7 @@ namespace gtsam {
* is the fastest.
*/
GenericSequentialSolver(const sharedFactorGraph& factorGraph,
const boost::shared_ptr<VariableIndex>& variableIndex);
const boost::shared_ptr<VariableIndexOrdered>& variableIndex);
/// @}
/// @name Testable
@ -158,7 +158,7 @@ namespace gtsam {
* Compute the marginal joint over a set of variables, by integrating out
* 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;
/**

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

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

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

@ -18,7 +18,7 @@
// \callgraph
#pragma once
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/BayesTreeOrdered.h>
namespace gtsam {
@ -28,11 +28,11 @@ namespace gtsam {
* \nosubgrouping
*/
template<class CONDITIONAL>
class ISAM: public BayesTree<CONDITIONAL> {
class ISAMOrdered: public BayesTreeOrdered<CONDITIONAL> {
private:
typedef BayesTree<CONDITIONAL> Base;
typedef BayesTreeOrdered<CONDITIONAL> Base;
public:
@ -40,10 +40,10 @@ namespace gtsam {
/// @{
/** Create an empty Bayes Tree */
ISAM();
ISAMOrdered();
/** Copy constructor */
ISAM(const Base& bayesTree) :
ISAMOrdered(const Base& bayesTree) :
Base(bayesTree) {
}
@ -59,8 +59,8 @@ namespace gtsam {
template<class FG>
void update(const FG& newFactors, typename FG::Eliminate function);
typedef typename BayesTree<CONDITIONAL>::sharedClique sharedClique; ///<TODO: comment
typedef typename BayesTree<CONDITIONAL>::Cliques Cliques; ///<TODO: comment
typedef typename BayesTreeOrdered<CONDITIONAL>::sharedClique sharedClique; ///<TODO: comment
typedef typename BayesTreeOrdered<CONDITIONAL>::Cliques Cliques; ///<TODO: comment
/** update_internal provides access to list of orphans for drawing purposes */
template<class FG>
@ -73,4 +73,4 @@ 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/inference/IndexConditional.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
@ -32,13 +32,13 @@ namespace gtsam {
using namespace boost::lambda;
/* ************************************************************************* */
void IndexConditional::assertInvariants() const {
void IndexConditionalOrdered::assertInvariants() const {
// Checks for uniqueness of keys
Base::assertInvariants();
}
/* ************************************************************************* */
bool IndexConditional::reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) {
bool IndexConditionalOrdered::reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
BOOST_FOREACH(KeyType key, frontals()) { assert(inverseReduction.find(key) == inverseReduction.end()); }
#endif
@ -55,7 +55,7 @@ namespace gtsam {
}
/* ************************************************************************* */
void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) {
void IndexConditionalOrdered::permuteWithInverse(const Permutation& inversePermutation) {
BOOST_FOREACH(Index& key, keys())
key = inversePermutation[key];
assertInvariants();

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

@ -18,9 +18,9 @@
#pragma once
#include <gtsam/global_includes.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/Permutation.h>
#include <gtsam/inference/ConditionalOrdered.h>
#include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/PermutationOrdered.h>
namespace gtsam {
@ -33,7 +33,7 @@ namespace gtsam {
* unsigned integer.
* \nosubgrouping
*/
class IndexConditional : public Conditional<Index> {
class IndexConditionalOrdered : public ConditionalOrdered<Index> {
protected:
@ -42,46 +42,46 @@ namespace gtsam {
public:
typedef IndexConditional This;
typedef Conditional<Index> Base;
typedef IndexFactor FactorType;
typedef boost::shared_ptr<IndexConditional> shared_ptr;
typedef IndexConditionalOrdered This;
typedef ConditionalOrdered<Index> Base;
typedef IndexFactorOrdered FactorType;
typedef boost::shared_ptr<IndexConditionalOrdered> shared_ptr;
/// @name Standard Constructors
/// @{
/** Empty Constructor to make serialization possible */
IndexConditional() { assertInvariants(); }
IndexConditionalOrdered() { assertInvariants(); }
/** No parents */
IndexConditional(Index j) : Base(j) { assertInvariants(); }
IndexConditionalOrdered(Index j) : Base(j) { assertInvariants(); }
/** Single parent */
IndexConditional(Index j, Index parent) : Base(j, parent) { assertInvariants(); }
IndexConditionalOrdered(Index j, Index parent) : Base(j, parent) { assertInvariants(); }
/** 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 */
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
/// @{
/** 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();
}
/** 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) {
assertInvariants();
}
/** 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) {
assertInvariants();
}
@ -93,15 +93,15 @@ namespace gtsam {
/** Named constructor directly returning a shared pointer */
template<class KEYS>
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->nrFrontals_ = nrFrontals;
return conditional;
}
/** Convert to a factor */
IndexFactor::shared_ptr toFactor() const {
return IndexFactor::shared_ptr(new IndexFactor(*this));
IndexFactorOrdered::shared_ptr toFactor() const {
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
*/
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/FactorOrdered.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
void IndexFactor::assertInvariants() const {
void IndexFactorOrdered::assertInvariants() const {
Base::assertInvariants();
}
/* ************************************************************************* */
IndexFactor::IndexFactor(const IndexConditional& c) :
IndexFactorOrdered::IndexFactorOrdered(const IndexConditionalOrdered& c) :
Base(c) {
assertInvariants();
}
/* ************************************************************************* */
#ifdef TRACK_ELIMINATE
boost::shared_ptr<IndexConditional> IndexFactor::eliminateFirst() {
boost::shared_ptr<IndexConditional> result(Base::eliminateFirst<
IndexConditional>());
boost::shared_ptr<IndexConditionalOrdered> IndexFactorOrdered::eliminateFirst() {
boost::shared_ptr<IndexConditionalOrdered> result(Base::eliminateFirst<
IndexConditionalOrdered>());
assertInvariants();
return result;
}
/* ************************************************************************* */
boost::shared_ptr<BayesNet<IndexConditional> > IndexFactor::eliminate(
boost::shared_ptr<BayesNetOrdered<IndexConditionalOrdered> > IndexFactorOrdered::eliminate(
size_t nrFrontals) {
boost::shared_ptr<BayesNet<IndexConditional> > result(Base::eliminate<
IndexConditional>(nrFrontals));
boost::shared_ptr<BayesNetOrdered<IndexConditionalOrdered> > result(Base::eliminate<
IndexConditionalOrdered>(nrFrontals));
assertInvariants();
return result;
}
#endif
/* ************************************************************************* */
void IndexFactor::permuteWithInverse(const Permutation& inversePermutation) {
void IndexFactorOrdered::permuteWithInverse(const Permutation& inversePermutation) {
BOOST_FOREACH(Index& key, keys())
key = inversePermutation[key];
assertInvariants();
}
/* ************************************************************************* */
void IndexFactor::reduceWithInverse(const internal::Reduction& inverseReduction) {
void IndexFactorOrdered::reduceWithInverse(const internal::Reduction& inverseReduction) {
BOOST_FOREACH(Index& key, keys())
key = inverseReduction[key];
assertInvariants();

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

@ -17,13 +17,13 @@
#pragma once
#include <gtsam/inference/Factor.h>
#include <gtsam/inference/Permutation.h>
#include <gtsam/inference/FactorOrdered.h>
#include <gtsam/inference/PermutationOrdered.h>
namespace gtsam {
// Forward declaration of IndexConditional
class IndexConditional;
class IndexConditionalOrdered;
/**
* 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.
* \nosubgrouping
*/
class IndexFactor: public Factor<Index> {
class IndexFactorOrdered: public FactorOrdered<Index> {
protected:
@ -48,64 +48,64 @@ namespace gtsam {
public:
typedef IndexFactor This;
typedef Factor<Index> Base;
typedef IndexFactorOrdered This;
typedef FactorOrdered<Index> Base;
/** Elimination produces an IndexConditional */
typedef IndexConditional ConditionalType;
typedef IndexConditionalOrdered ConditionalType;
/** Overriding the shared_ptr typedef */
typedef boost::shared_ptr<IndexFactor> shared_ptr;
typedef boost::shared_ptr<IndexFactorOrdered> shared_ptr;
/// @name Standard Interface
/// @{
/** Copy constructor */
IndexFactor(const This& f) :
IndexFactorOrdered(const This& f) :
Base(f) {
assertInvariants();
}
/** Construct from derived type */
GTSAM_EXPORT IndexFactor(const IndexConditional& c);
GTSAM_EXPORT IndexFactorOrdered(const IndexConditionalOrdered& c);
/** Default constructor for I/O */
IndexFactor() {
IndexFactorOrdered() {
assertInvariants();
}
/** Construct unary factor */
IndexFactor(Index j) :
IndexFactorOrdered(Index j) :
Base(j) {
assertInvariants();
}
/** Construct binary factor */
IndexFactor(Index j1, Index j2) :
IndexFactorOrdered(Index j1, Index j2) :
Base(j1, j2) {
assertInvariants();
}
/** Construct ternary factor */
IndexFactor(Index j1, Index j2, Index j3) :
IndexFactorOrdered(Index j1, Index j2, Index j3) :
Base(j1, j2, j3) {
assertInvariants();
}
/** 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) {
assertInvariants();
}
/** 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) {
assertInvariants();
}
/** 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) {
assertInvariants();
}
@ -115,19 +115,19 @@ namespace gtsam {
/// @{
/** Construct n-way factor */
IndexFactor(const std::set<Index>& js) :
IndexFactorOrdered(const std::set<Index>& js) :
Base(js) {
assertInvariants();
}
/** Construct n-way factor */
IndexFactor(const std::vector<Index>& js) :
IndexFactorOrdered(const std::vector<Index>& js) :
Base(js) {
assertInvariants();
}
/** Constructor from a collection of keys */
template<class KeyIterator> IndexFactor(KeyIterator beginKey,
template<class KeyIterator> IndexFactorOrdered(KeyIterator beginKey,
KeyIterator endKey) :
Base(beginKey, endKey) {
assertInvariants();
@ -143,7 +143,7 @@ namespace gtsam {
GTSAM_EXPORT boost::shared_ptr<ConditionalType> eliminateFirst();
/** 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);
#endif
@ -161,7 +161,7 @@ namespace gtsam {
*/
GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction);
virtual ~IndexFactor() {
virtual ~IndexFactorOrdered() {
}
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/treeTraversal-inst.h>
#include <gtsam/inference/JunctionTreeUnordered.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/symbolic/SymbolicConditional.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <boost/foreach.hpp>
#include <boost/bind.hpp>
@ -36,9 +36,9 @@ namespace gtsam {
template<class BAYESTREE, class GRAPH>
struct ConstructorTraversalData {
ConstructorTraversalData* const parentData;
typename JunctionTreeUnordered<BAYESTREE,GRAPH>::sharedNode myJTNode;
std::vector<SymbolicConditionalUnordered::shared_ptr> childSymbolicConditionals;
std::vector<SymbolicFactorUnordered::shared_ptr> childSymbolicFactors;
typename JunctionTree<BAYESTREE,GRAPH>::sharedNode myJTNode;
std::vector<SymbolicConditional::shared_ptr> childSymbolicConditionals;
std::vector<SymbolicFactor::shared_ptr> childSymbolicFactors;
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
// structure with its own JT node, and create a child pointer in its parent.
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->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end());
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.
// Do symbolic elimination for this node
SymbolicFactorGraphUnordered symbolicFactors;
SymbolicFactorGraph symbolicFactors;
symbolicFactors.reserve(ETreeNode->factors.size() + myData.childSymbolicFactors.size());
// Add symbolic versions of the ETree node factors
BOOST_FOREACH(const typename GRAPH::sharedFactor& factor, ETreeNode->factors) {
symbolicFactors.push_back(boost::make_shared<SymbolicFactorUnordered>(
SymbolicFactorUnordered::FromKeys(*factor))); }
symbolicFactors.push_back(boost::make_shared<SymbolicFactor>(
SymbolicFactor::FromKeys(*factor))); }
// Add symbolic factors passed up from children
symbolicFactors.push_back(myData.childSymbolicFactors.begin(), myData.childSymbolicFactors.end());
OrderingUnordered keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key);
std::pair<SymbolicConditionalUnordered::shared_ptr, SymbolicFactorUnordered::shared_ptr> symbolicElimResult =
EliminateSymbolicUnordered(symbolicFactors, keyAsOrdering);
Ordering keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key);
std::pair<SymbolicConditional::shared_ptr, SymbolicFactor::shared_ptr> symbolicElimResult =
EliminateSymbolic(symbolicFactors, keyAsOrdering);
// Store symbolic elimination results in the parent
myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first);
@ -102,7 +102,7 @@ namespace gtsam {
if(myNrParents + 1 == myData.childSymbolicConditionals[child]->nrParents()) {
// Get a reference to the child, adjusting the index to account for children previously
// 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];
// Merge keys, factors, and children.
myData.myJTNode->keys.insert(myData.myJTNode->keys.begin(), childToMerge.keys.begin(), childToMerge.keys.end());
@ -182,7 +182,7 @@ namespace gtsam {
// Do dense elimination step
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
myData.bayesTreeNode->conditional_ = eliminationResult.first;
@ -195,7 +195,7 @@ namespace gtsam {
/* ************************************************************************* */
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
{
std::cout << s;
@ -206,7 +206,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class BAYESTREE, class GRAPH>
void JunctionTreeUnordered<BAYESTREE,GRAPH>::print(
void JunctionTree<BAYESTREE,GRAPH>::print(
const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const
{
treeTraversal::PrintForest(*this, s, keyFormatter);
@ -215,8 +215,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class BAYESTREE, class GRAPH>
template<class ETREE>
JunctionTreeUnordered<BAYESTREE,GRAPH>
JunctionTreeUnordered<BAYESTREE,GRAPH>::FromEliminationTree(const ETREE& eliminationTree)
JunctionTree<BAYESTREE,GRAPH>
JunctionTree<BAYESTREE,GRAPH>::FromEliminationTree(const ETREE& eliminationTree)
{
gttic(JunctionTree_FromEliminationTree);
// 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>
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.
roots_ = treeTraversal::CloneForest(other);
@ -260,7 +260,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class BAYESTREE, class 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);
// 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
* @author Kai Ni
* @author Frank Dellaert
* @brief: The junction tree
* @author Richard Roberts
* @brief The junction tree
*/
#pragma once
#include <set>
#include <vector>
#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>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
namespace gtsam {
@ -46,90 +42,103 @@ namespace gtsam {
* 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 BayesTree<typename FG::FactorType::ConditionalType>::Clique>
class JunctionTree: public ClusterTree<FG> {
template<class BAYESTREE, class GRAPH>
class JunctionTree {
public:
/// In a junction tree each cluster is associated with a clique
typedef typename ClusterTree<FG>::Cluster Clique;
typedef typename Clique::shared_ptr sharedClique; ///< Shared pointer to a clique
typedef GRAPH FactorGraphType; ///< The factor graph type
typedef typename GRAPH::FactorType FactorType; ///< The type of factors
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
typedef BTCLIQUE BTClique;
struct Node {
typedef std::vector<Key> Keys;
typedef std::vector<sharedFactor> Factors;
typedef std::vector<boost::shared_ptr<Node> > Children;
/// Shared pointer to this class
typedef boost::shared_ptr<JunctionTree<FG> > shared_ptr;
Keys keys; ///< Frontal keys of this node
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
typedef gtsam::BayesTree<IndexConditional> SymbolicBayesTree;
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:
/// @name Advanced Interface
/// @{
/** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
/// distribute the factors along the cluster tree
sharedClique distributeFactors(const FG& fg,
const SymbolicBayesTree::sharedClique& clique);
std::vector<sharedNode> roots_;
std::vector<sharedFactor> remainingFactors_;
/// 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 VariableIndex& variableIndex);
/// @}
public:
protected:
/// @name Standard Constructors
/// @{
/** Default constructor */
JunctionTree() {}
/** 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. */
JunctionTree(const This& other) { *this = other; }
/** 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
*/
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);
/** 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 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;
/** 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;
/// @}
}; // 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
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/inference/EliminationTreeOrdered.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
@ -30,15 +30,15 @@ namespace gtsam {
/* ************************************************************************* */
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_symbolic_ET);
const typename EliminationTree<IndexFactor>::shared_ptr symETree =
EliminationTree<IndexFactor>::Create(fg, variableIndex);
const typename EliminationTreeOrdered<IndexFactorOrdered>::shared_ptr symETree =
EliminationTreeOrdered<IndexFactorOrdered>::Create(fg, variableIndex);
assert(symETree.get());
gttoc(JT_symbolic_ET);
gttic(JT_symbolic_eliminate);
SymbolicBayesNet::shared_ptr sbn = symETree->eliminate(&EliminateSymbolic);
SymbolicBayesNetOrdered::shared_ptr sbn = symETree->eliminate(&EliminateSymbolic);
assert(sbn.get());
gttoc(JT_symbolic_eliminate);
gttic(symbolic_BayesTree);
@ -53,22 +53,22 @@ namespace gtsam {
/* ************************************************************************* */
template <class FG, class BTCLIQUE>
JunctionTree<FG,BTCLIQUE>::JunctionTree(const FG& fg) {
gttic(VariableIndex);
VariableIndex varIndex(fg);
gttoc(VariableIndex);
JunctionTreeOrdered<FG,BTCLIQUE>::JunctionTreeOrdered(const FG& fg) {
gttic(VariableIndexOrdered);
VariableIndexOrdered varIndex(fg);
gttoc(VariableIndexOrdered);
construct(fg, varIndex);
}
/* ************************************************************************* */
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);
}
/* ************************************************************************* */
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) {
// Build "target" index. This is an index for each variable of the factors
@ -104,7 +104,7 @@ namespace gtsam {
/* ************************************************************************* */
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 SymbolicBayesTree::sharedClique& bayesClique) {
@ -142,8 +142,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class FG, class BTCLIQUE>
std::pair<typename JunctionTree<FG,BTCLIQUE>::BTClique::shared_ptr,
typename FG::sharedFactor> JunctionTree<FG,BTCLIQUE>::eliminateOneClique(
std::pair<typename JunctionTreeOrdered<FG,BTCLIQUE>::BTClique::shared_ptr,
typename FG::sharedFactor> JunctionTreeOrdered<FG,BTCLIQUE>::eliminateOneClique(
typename FG::Eliminate function,
const boost::shared_ptr<const Clique>& current) const {
@ -189,7 +189,7 @@ namespace gtsam {
/* ************************************************************************* */
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
{
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 <limits>
#include <gtsam/inference/OrderingUnordered.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>
using namespace std;
@ -28,7 +28,7 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
FastMap<Key, size_t> OrderingUnordered::invert() const
FastMap<Key, size_t> Ordering::invert() const
{
FastMap<Key, size_t> inverted;
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
return OrderingUnordered::COLAMDConstrained(variableIndex, vector<int>(variableIndex.size(), 0));
return Ordering::COLAMDConstrained(variableIndex, vector<int>(variableIndex.size(), 0));
}
/* ************************************************************************* */
OrderingUnordered OrderingUnordered::COLAMDConstrained(
const VariableIndexUnordered& variableIndex, std::vector<int>& cmember)
Ordering Ordering::COLAMDConstrained(
const VariableIndex& variableIndex, std::vector<int>& cmember)
{
gttic(OrderingUnordered_COLAMDConstrained);
gttic(Ordering_COLAMDConstrained);
gttic(Prepare);
size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
@ -61,9 +61,9 @@ namespace gtsam {
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
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
const VariableIndexUnordered::Factors& column = key_factors.second;
const VariableIndex::Factors& column = key_factors.second;
size_t lastFactorId = numeric_limits<size_t>::max();
BOOST_FOREACH(size_t factorIndex, column) {
if(lastFactorId != numeric_limits<size_t>::max())
@ -101,7 +101,7 @@ namespace gtsam {
gttic(Fill_Ordering);
// Convert elimination ordering in p to an ordering
OrderingUnordered result;
Ordering result;
result.resize(nVars);
for(size_t j = 0; j < nVars; ++j)
result[j] = keys[p[j]];
@ -111,10 +111,10 @@ namespace gtsam {
}
/* ************************************************************************* */
OrderingUnordered OrderingUnordered::COLAMDConstrainedLast(
const VariableIndexUnordered& variableIndex, const std::vector<Key>& constrainLast, bool forceOrder)
Ordering Ordering::COLAMDConstrainedLast(
const VariableIndex& variableIndex, const std::vector<Key>& constrainLast, bool forceOrder)
{
gttic(OrderingUnordered_COLAMDConstrainedLast);
gttic(Ordering_COLAMDConstrainedLast);
size_t n = variableIndex.size();
std::vector<int> cmember(n, 0);
@ -122,7 +122,7 @@ namespace gtsam {
// Build a mapping to look up sorted Key indices by Key
FastMap<Key, size_t> keyIndices;
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++));
// If at least some variables are not constrained to be last, constrain the
@ -134,7 +134,7 @@ namespace gtsam {
++ 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 <gtsam/inference/Key.h>
#include <gtsam/inference/VariableIndexUnordered.h>
#include <gtsam/inference/FactorGraphUnordered.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/FactorGraph.h>
namespace gtsam {
class OrderingUnordered : public std::vector<Key> {
class Ordering : public std::vector<Key> {
protected:
typedef std::vector<Key> Base;
public:
/// Create an empty ordering
GTSAM_EXPORT OrderingUnordered() {}
GTSAM_EXPORT Ordering() {}
/// Create from a container
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
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
FastMap<Key, size_t> invert() const;
/// 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
/// VariableIndexUnordered&)
/// VariableIndex&)
template<class FACTOR>
static OrderingUnordered COLAMD(const FactorGraphUnordered<FACTOR>& graph) {
return COLAMD(VariableIndexUnordered(graph));
static Ordering COLAMD(const FactorGraph<FACTOR>& 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(
const VariableIndexUnordered& variableIndex, const std::vector<Key>& constrainLast, bool forceOrder = false);
static GTSAM_EXPORT Ordering COLAMDConstrainedLast(
const VariableIndex& variableIndex, const std::vector<Key>& constrainLast, bool forceOrder = false);
private:
static GTSAM_EXPORT OrderingUnordered COLAMDConstrained(
const VariableIndexUnordered& variableIndex,
static GTSAM_EXPORT Ordering COLAMDConstrained(
const VariableIndex& variableIndex,
std::vector<int>& cmember);
};
}

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

@ -15,7 +15,7 @@
* @date Oct 9, 2010
*/
#include <gtsam/inference/Permutation.h>
#include <gtsam/inference/PermutationOrdered.h>
#include <iostream>
#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
#include <gtsam/global_includes.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/FactorGraphOrdered.h>
#include <gtsam/inference/IndexFactorOrdered.h>
namespace gtsam { template<class FACTOR> class EliminationTree; }
namespace gtsam { template<class CONDITIONAL> class BayesNet; }
namespace gtsam { template<class CONDITIONAL, class CLIQUE> class BayesTree; }
namespace gtsam { class IndexConditional; }
namespace gtsam { template<class FACTOR> class EliminationTreeOrdered; }
namespace gtsam { template<class CONDITIONAL> class BayesNetOrdered; }
namespace gtsam { template<class CONDITIONAL, class CLIQUE> class BayesTreeOrdered; }
namespace gtsam { class IndexConditionalOrdered; }
namespace gtsam {
typedef EliminationTree<IndexFactor> SymbolicEliminationTree;
typedef BayesNet<IndexConditional> SymbolicBayesNet;
typedef BayesTree<IndexConditional> SymbolicBayesTree;
typedef EliminationTreeOrdered<IndexFactorOrdered> SymbolicEliminationTreeOrdered;
typedef BayesNetOrdered<IndexConditionalOrdered> SymbolicBayesNetOrdered;
typedef BayesTreeOrdered<IndexConditionalOrdered> SymbolicBayesTreeOrdered;
/** Symbolic IndexFactor Graph
* \nosubgrouping
*/
class SymbolicFactorGraph: public FactorGraph<IndexFactor> {
class SymbolicFactorGraphOrdered: public FactorGraphOrdered<IndexFactorOrdered> {
public:
@ -43,29 +43,29 @@ namespace gtsam {
/// @{
/** Construct empty factor graph */
SymbolicFactorGraph() {
SymbolicFactorGraphOrdered() {
}
/** Construct from a BayesNet */
GTSAM_EXPORT SymbolicFactorGraph(const SymbolicBayesNet& bayesNet);
GTSAM_EXPORT SymbolicFactorGraphOrdered(const SymbolicBayesNetOrdered& bayesNet);
/** Construct from a BayesTree */
GTSAM_EXPORT SymbolicFactorGraph(const SymbolicBayesTree& bayesTree);
GTSAM_EXPORT SymbolicFactorGraphOrdered(const SymbolicBayesTreeOrdered& bayesTree);
/**
* Construct from a factor graph of any type
*/
template<class FACTOR>
SymbolicFactorGraph(const FactorGraph<FACTOR>& fg);
/** 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. Note that this version simply calls
* FactorGraph<IndexFactor>::eliminateFrontals with EliminateSymbolic
* as the eliminate function argument.
*/
GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> eliminateFrontals(size_t nFrontals) const;
SymbolicFactorGraphOrdered(const FactorGraphOrdered<FACTOR>& fg);
/** 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. Note that this version simply calls
* FactorGraph<IndexFactor>::eliminateFrontals with EliminateSymbolic
* as the eliminate function argument.
*/
GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraphOrdered> eliminateFrontals(size_t nFrontals) 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
@ -79,14 +79,14 @@ namespace gtsam {
* 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.
* Note that this version simply calls
* Note that this version simply calls
* FactorGraph<GaussianFactor>::eliminate with EliminateSymbolic as the eliminate
* 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. */
GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> eliminateOne(Index variable) const;
GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraphOrdered> eliminateOne(Index variable) const;
/// @}
/// @name Standard Interface
@ -113,17 +113,17 @@ namespace gtsam {
/** Push back 4-way factor */
GTSAM_EXPORT void push_factor(Index key1, Index key2, Index key3, Index key4);
/** Permute the variables in the factors */
GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation);
/** Permute the variables in the factors */
GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation);
/** Apply a reduction, which is a remapping of variable indices. */
GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction);
};
/** 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);
/**
@ -131,20 +131,20 @@ namespace gtsam {
* Combine and eliminate can also be called separately, but for this and
* derived classes calling them separately generally does extra work.
*/
GTSAM_EXPORT std::pair<boost::shared_ptr<IndexConditional>, boost::shared_ptr<IndexFactor> >
EliminateSymbolic(const FactorGraph<IndexFactor>&, size_t nrFrontals = 1);
GTSAM_EXPORT std::pair<boost::shared_ptr<IndexConditionalOrdered>, boost::shared_ptr<IndexFactorOrdered> >
EliminateSymbolic(const FactorGraphOrdered<IndexFactorOrdered>&, size_t nrFrontals = 1);
/// @}
/** Template function implementation */
template<class FACTOR>
SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph<FACTOR>& fg) {
SymbolicFactorGraphOrdered::SymbolicFactorGraphOrdered(const FactorGraphOrdered<FACTOR>& fg) {
for (size_t i = 0; i < fg.size(); i++) {
if (fg[i]) {
IndexFactor::shared_ptr factor(new IndexFactor(*fg[i]));
IndexFactorOrdered::shared_ptr factor(new IndexFactorOrdered(*fg[i]));
push_back(factor);
} 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
*/
#include <gtsam/inference/SymbolicMultifrontalSolver.h>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/inference/SymbolicMultifrontalSolverOrdered.h>
#include <gtsam/inference/JunctionTreeOrdered.h>
namespace gtsam {

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

@ -18,14 +18,14 @@
#pragma once
#include <gtsam/inference/GenericMultifrontalSolver.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
namespace gtsam {
class SymbolicMultifrontalSolver : GenericMultifrontalSolver<IndexFactor, JunctionTree<FactorGraph<IndexFactor> > > {
class SymbolicMultifrontalSolver : GenericMultifrontalSolver<IndexFactorOrdered, JunctionTreeOrdered<FactorGraphOrdered<IndexFactorOrdered> > > {
protected:
typedef GenericMultifrontalSolver<IndexFactor, JunctionTree<FactorGraph<IndexFactor> > > Base;
typedef GenericMultifrontalSolver<IndexFactorOrdered, JunctionTreeOrdered<FactorGraphOrdered<IndexFactorOrdered> > > Base;
public:
/**
@ -33,15 +33,15 @@ namespace gtsam {
* tree, which does the symbolic elimination, identifies the 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
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
SymbolicMultifrontalSolver(const SymbolicFactorGraph::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {};
SymbolicMultifrontalSolver(const SymbolicFactorGraphOrdered::shared_ptr& factorGraph,
const VariableIndexOrdered::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {};
/** Print to cout */
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
* 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
* 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
* 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
*/
#include <gtsam/inference/SymbolicSequentialSolver.h>
#include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
#include <gtsam/inference/GenericSequentialSolver-inl.h>
namespace gtsam {

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

@ -17,14 +17,14 @@
#pragma once
#include <gtsam/inference/GenericSequentialSolver.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
namespace gtsam {
class SymbolicSequentialSolver : GenericSequentialSolver<IndexFactor> {
class SymbolicSequentialSolver : GenericSequentialSolver<IndexFactorOrdered> {
protected:
typedef GenericSequentialSolver<IndexFactor> Base;
typedef GenericSequentialSolver<IndexFactorOrdered> Base;
public:
/**
@ -32,15 +32,15 @@ namespace gtsam {
* tree, which does the symbolic elimination, identifies the 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
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
SymbolicSequentialSolver(const SymbolicFactorGraph::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {};
SymbolicSequentialSolver(const SymbolicFactorGraphOrdered::shared_ptr& factorGraph,
const VariableIndexOrdered::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {};
/** Print to cout */
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
* to recursively eliminate.
*/
SymbolicBayesNet::shared_ptr eliminate() const
SymbolicBayesNetOrdered::shared_ptr eliminate() const
{ 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)
* 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); };
/**
* Compute the marginal joint over a set of variables, by integrating out
* 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); };
/**
* Compute the marginal joint over a set of variables, by integrating out
* 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); };
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* 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
#include <gtsam/inference/VariableIndexUnordered.h>
#include <gtsam/inference/VariableIndex.h>
namespace gtsam {
/* ************************************************************************* */
template<class FG>
void VariableIndexUnordered::augment(const FG& factors)
void VariableIndex::augment(const FG& factors)
{
gttic(VariableIndex_augment);
@ -45,7 +45,7 @@ void VariableIndexUnordered::augment(const FG& factors)
/* ************************************************************************* */
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);
@ -73,7 +73,7 @@ void VariableIndexUnordered::remove(ITERATOR firstFactor, ITERATOR lastFactor, c
/* ************************************************************************* */
template<typename ITERATOR>
void VariableIndexUnordered::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) {
void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) {
for(ITERATOR key = firstKey; key != lastKey; ++key) {
assert(internalAt(*key).empty());
index_.erase(*key);

162
gtsam/inference/VariableIndex.cpp
View File

@ -1,104 +1,58 @@
/* ----------------------------------------------------------------------------
* 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/VariableIndex.h>
#include <gtsam/inference/Permutation.h>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
bool VariableIndex::equals(const VariableIndex& 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 VariableIndex::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 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);
}
}
/* ----------------------------------------------------------------------------
* 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/VariableIndex.h>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
bool VariableIndex::equals(const VariableIndex& other, double tol) const {
return this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_
&& this->index_ == other.index_;
}
/* ************************************************************************* */
void VariableIndex::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 VariableIndex::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;
}
}

474
gtsam/inference/VariableIndex.h
View File

@ -1,292 +1,182 @@
/* ----------------------------------------------------------------------------
* 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 VariableIndex {
public:
typedef boost::shared_ptr<VariableIndex> 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 */
VariableIndex() : 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> VariableIndex(const FG& factorGraph, Index nVariables);
/**
* Create a VariableIndex that computes and stores the block column structure
* of a factor graph.
*/
template<class FG> VariableIndex(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 VariableIndex& 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
VariableIndex(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 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]) {
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_;
}
}
}
}
/* ----------------------------------------------------------------------------
* 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 VariableIndex {
public:
typedef boost::shared_ptr<VariableIndex> 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 */
VariableIndex() : nFactors_(0), nEntries_(0) {}
/**
* Create a VariableIndex that computes and stores the block column structure
* of a factor graph.
*/
template<class FG>
VariableIndex(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 VariableIndex& 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/VariableIndex-inl.h>

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>
// 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>
@ -32,7 +32,7 @@ namespace inference {
/* ************************************************************************* */
template<typename CONSTRAINED>
Permutation::shared_ptr PermutationCOLAMD(
const VariableIndex& variableIndex, const CONSTRAINED& constrainLast, bool forceOrder) {
const VariableIndexOrdered& variableIndex, const CONSTRAINED& constrainLast, bool forceOrder) {
gttic(PermutationCOLAMD_constrained);
size_t n = variableIndex.size();
@ -59,7 +59,7 @@ Permutation::shared_ptr PermutationCOLAMD(
/* ************************************************************************* */
template<typename CONSTRAINED_MAP>
Permutation::shared_ptr PermutationCOLAMDGrouped(
const VariableIndex& variableIndex, const CONSTRAINED_MAP& constraints) {
const VariableIndexOrdered& variableIndex, const CONSTRAINED_MAP& constraints) {
gttic(PermutationCOLAMD_grouped);
size_t n = variableIndex.size();
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);
size_t n = variableIndex.size();
std::vector<int> cmember(n, 0);

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

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

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

@ -19,8 +19,8 @@
#pragma once
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/Permutation.h>
#include <gtsam/inference/VariableIndexOrdered.h>
#include <gtsam/inference/PermutationOrdered.h>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
@ -35,7 +35,7 @@ namespace gtsam {
* Compute a permutation (variable ordering) using colamd
*/
GTSAM_EXPORT Permutation::shared_ptr PermutationCOLAMD(
const VariableIndex& variableIndex);
const VariableIndexOrdered& variableIndex);
/**
* Compute a permutation (variable ordering) using constrained colamd to move
@ -47,7 +47,7 @@ namespace gtsam {
*/
template<typename CONSTRAINED>
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
@ -59,7 +59,7 @@ namespace gtsam {
*/
template<typename CONSTRAINED_MAP>
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.
@ -73,10 +73,10 @@ namespace gtsam {
* AGC: does cmember change?
*/
GTSAM_EXPORT Permutation::shared_ptr PermutationCOLAMD_(
const VariableIndex& variableIndex, std::vector<int>& cmember);
const VariableIndexOrdered& variableIndex, std::vector<int>& cmember);
} // \namespace inference
} // \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 <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/SymbolicSequentialSolver.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/BayesTreeOrdered.h>
#include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
using namespace std;
using namespace gtsam;
@ -38,18 +38,18 @@ using namespace gtsam;
//// SLAM example from RSS sqrtSAM paper
static const Index _x3_=0, _x2_=1;
//static const Index _x1_=2, _l2_=3, _l1_=4; // unused
//IndexConditional::shared_ptr
// x3(new IndexConditional(_x3_)),
// x2(new IndexConditional(_x2_,_x3_)),
// x1(new IndexConditional(_x1_,_x2_,_x3_)),
// l1(new IndexConditional(_l1_,_x1_,_x2_)),
// l2(new IndexConditional(_l2_,_x1_,_x3_));
//IndexConditionalOrdered::shared_ptr
// x3(new IndexConditionalOrdered(_x3_)),
// x2(new IndexConditionalOrdered(_x2_,_x3_)),
// x1(new IndexConditionalOrdered(_x1_,_x2_,_x3_)),
// l1(new IndexConditionalOrdered(_l1_,_x1_,_x2_)),
// l2(new IndexConditionalOrdered(_l2_,_x1_,_x3_));
//
//// Bayes Tree for sqrtSAM example
//SymbolicBayesTree createSlamSymbolicBayesTree(){
//SymbolicBayesTreeOrdered createSlamSymbolicBayesTree(){
// // Create using insert
//// Ordering slamOrdering; slamOrdering += _x3_, _x2_, _x1_, _l2_, _l1_;
// SymbolicBayesTree bayesTree_slam;
// SymbolicBayesTreeOrdered bayesTree_slam;
// bayesTree_slam.insert(x3);
// bayesTree_slam.insert(x2);
// 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
static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5;
static IndexConditional::shared_ptr
B(new IndexConditional(_B_)),
L(new IndexConditional(_L_, _B_)),
E(new IndexConditional(_E_, _L_, _B_)),
S(new IndexConditional(_S_, _L_, _B_)),
T(new IndexConditional(_T_, _E_, _L_)),
X(new IndexConditional(_X_, _E_));
static IndexConditionalOrdered::shared_ptr
B(new IndexConditionalOrdered(_B_)),
L(new IndexConditionalOrdered(_L_, _B_)),
E(new IndexConditionalOrdered(_E_, _L_, _B_)),
S(new IndexConditionalOrdered(_S_, _L_, _B_)),
T(new IndexConditionalOrdered(_T_, _E_, _L_)),
X(new IndexConditionalOrdered(_X_, _E_));
// Cliques
static IndexConditional::shared_ptr
ELB(IndexConditional::FromKeys(cref_list_of<3>(_E_)(_L_)(_B_), 3));
static IndexConditionalOrdered::shared_ptr
ELB(IndexConditionalOrdered::FromKeys(cref_list_of<3>(_E_)(_L_)(_B_), 3));
// Bayes Tree for Asia example
static SymbolicBayesTree createAsiaSymbolicBayesTree() {
SymbolicBayesTree bayesTree;
static SymbolicBayesTreeOrdered createAsiaSymbolicBayesTree() {
SymbolicBayesTreeOrdered bayesTree;
// Ordering asiaOrdering; asiaOrdering += _X_, _T_, _S_, _E_, _L_, _B_;
SymbolicBayesTree::insert(bayesTree, B);
SymbolicBayesTree::insert(bayesTree, L);
SymbolicBayesTree::insert(bayesTree, E);
SymbolicBayesTree::insert(bayesTree, S);
SymbolicBayesTree::insert(bayesTree, T);
SymbolicBayesTree::insert(bayesTree, X);
SymbolicBayesTreeOrdered::insert(bayesTree, B);
SymbolicBayesTreeOrdered::insert(bayesTree, L);
SymbolicBayesTreeOrdered::insert(bayesTree, E);
SymbolicBayesTreeOrdered::insert(bayesTree, S);
SymbolicBayesTreeOrdered::insert(bayesTree, T);
SymbolicBayesTreeOrdered::insert(bayesTree, X);
return bayesTree;
}
/* ************************************************************************* */
TEST( BayesTree, constructor )
TEST( BayesTreeOrdered, constructor )
{
// Create using insert
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree();
bayesTree.print("bayesTree (ordered): ");
@ -99,18 +99,18 @@ TEST( BayesTree, constructor )
EXPECT(!bayesTree.empty());
// 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));
// 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(T);
ASIA.push_back(S);
ASIA.push_back(E);
ASIA.push_back(L);
ASIA.push_back(B);
SymbolicBayesTree bayesTree2(ASIA);
SymbolicBayesTreeOrdered bayesTree2(ASIA);
// Check whether the same
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();
//
// 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));
}
@ -148,144 +148,144 @@ Bayes Tree for testing conversion to a forest of orphans needed for incremental.
D|C F|E
*/
/* ************************************************************************* */
TEST( BayesTree, removePath )
TEST( BayesTreeOrdered, removePath )
{
const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0;
IndexConditional::shared_ptr
A(new IndexConditional(_A_)),
B(new IndexConditional(_B_, _A_)),
C(new IndexConditional(_C_, _A_)),
D(new IndexConditional(_D_, _C_)),
E(new IndexConditional(_E_, _B_)),
F(new IndexConditional(_F_, _E_));
SymbolicBayesTree bayesTree;
IndexConditionalOrdered::shared_ptr
A(new IndexConditionalOrdered(_A_)),
B(new IndexConditionalOrdered(_B_, _A_)),
C(new IndexConditionalOrdered(_C_, _A_)),
D(new IndexConditionalOrdered(_D_, _C_)),
E(new IndexConditionalOrdered(_E_, _B_)),
F(new IndexConditionalOrdered(_F_, _E_));
SymbolicBayesTreeOrdered bayesTree;
EXPECT(bayesTree.empty());
// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_;
SymbolicBayesTree::insert(bayesTree, A);
SymbolicBayesTree::insert(bayesTree, B);
SymbolicBayesTree::insert(bayesTree, C);
SymbolicBayesTree::insert(bayesTree, D);
SymbolicBayesTree::insert(bayesTree, E);
SymbolicBayesTree::insert(bayesTree, F);
SymbolicBayesTreeOrdered::insert(bayesTree, A);
SymbolicBayesTreeOrdered::insert(bayesTree, B);
SymbolicBayesTreeOrdered::insert(bayesTree, C);
SymbolicBayesTreeOrdered::insert(bayesTree, D);
SymbolicBayesTreeOrdered::insert(bayesTree, E);
SymbolicBayesTreeOrdered::insert(bayesTree, F);
// remove C, expected outcome: factor graph with ABC,
// 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(_A_);
expected.push_factor(_C_,_A_);
SymbolicBayesTree::Cliques expectedOrphans;
SymbolicBayesTreeOrdered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_D_], bayesTree[_E_];
BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans;
BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTreeOrdered::Cliques orphans;
bayesTree.removePath(bayesTree[_C_], bn, orphans);
SymbolicFactorGraph factors(bn);
CHECK(assert_equal((SymbolicFactorGraph)expected, factors));
SymbolicFactorGraphOrdered factors(bn);
CHECK(assert_equal((SymbolicFactorGraphOrdered)expected, factors));
CHECK(assert_equal(expectedOrphans, orphans));
// remove E: factor graph with EB; E|B removed from second orphan tree
SymbolicFactorGraph expected2;
SymbolicFactorGraphOrdered expected2;
expected2.push_factor(_E_,_B_);
SymbolicBayesTree::Cliques expectedOrphans2;
SymbolicBayesTreeOrdered::Cliques expectedOrphans2;
expectedOrphans2 += bayesTree[_F_];
BayesNet<IndexConditional> bn2;
SymbolicBayesTree::Cliques orphans2;
BayesNetOrdered<IndexConditionalOrdered> bn2;
SymbolicBayesTreeOrdered::Cliques orphans2;
bayesTree.removePath(bayesTree[_E_], bn2, orphans2);
SymbolicFactorGraph factors2(bn2);
CHECK(assert_equal((SymbolicFactorGraph)expected2, factors2));
SymbolicFactorGraphOrdered factors2(bn2);
CHECK(assert_equal((SymbolicFactorGraphOrdered)expected2, factors2));
CHECK(assert_equal(expectedOrphans2, orphans2));
}
/* ************************************************************************* */
TEST( BayesTree, removePath2 )
TEST( BayesTreeOrdered, removePath2 )
{
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree();
// Call remove-path with clique B
BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans;
BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTreeOrdered::Cliques orphans;
bayesTree.removePath(bayesTree[_B_], bn, orphans);
SymbolicFactorGraph factors(bn);
SymbolicFactorGraphOrdered factors(bn);
// Check expected outcome
SymbolicFactorGraph expected;
SymbolicFactorGraphOrdered expected;
expected.push_factor(_E_,_L_,_B_);
// expected.push_factor(_L_,_B_);
// expected.push_factor(_B_);
CHECK(assert_equal(expected, factors));
SymbolicBayesTree::Cliques expectedOrphans;
SymbolicBayesTreeOrdered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_S_], bayesTree[_T_], bayesTree[_X_];
CHECK(assert_equal(expectedOrphans, orphans));
}
/* ************************************************************************* */
TEST( BayesTree, removePath3 )
TEST( BayesTreeOrdered, removePath3 )
{
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree();
// Call remove-path with clique S
BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans;
BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTreeOrdered::Cliques orphans;
bayesTree.removePath(bayesTree[_S_], bn, orphans);
SymbolicFactorGraph factors(bn);
SymbolicFactorGraphOrdered factors(bn);
// Check expected outcome
SymbolicFactorGraph expected;
SymbolicFactorGraphOrdered expected;
expected.push_factor(_E_,_L_,_B_);
// expected.push_factor(_L_,_B_);
// expected.push_factor(_B_);
expected.push_factor(_S_,_L_,_B_);
CHECK(assert_equal(expected, factors));
SymbolicBayesTree::Cliques expectedOrphans;
SymbolicBayesTreeOrdered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_T_], bayesTree[_X_];
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
if (subtree) {
cliques.push_back(subtree);
// Recursive call over all child cliques
BOOST_FOREACH(SymbolicBayesTree::sharedClique& childClique, subtree->children()) {
BOOST_FOREACH(SymbolicBayesTreeOrdered::sharedClique& childClique, subtree->children()) {
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;
IndexConditional::shared_ptr
A(new IndexConditional(_A_)),
B(new IndexConditional(_B_, _A_)),
C(new IndexConditional(_C_, _A_)),
D(new IndexConditional(_D_, _C_)),
E(new IndexConditional(_E_, _B_)),
F(new IndexConditional(_F_, _E_)),
G(new IndexConditional(_G_, _F_));
SymbolicBayesTree bayesTree;
IndexConditionalOrdered::shared_ptr
A(new IndexConditionalOrdered(_A_)),
B(new IndexConditionalOrdered(_B_, _A_)),
C(new IndexConditionalOrdered(_C_, _A_)),
D(new IndexConditionalOrdered(_D_, _C_)),
E(new IndexConditionalOrdered(_E_, _B_)),
F(new IndexConditionalOrdered(_F_, _E_)),
G(new IndexConditionalOrdered(_G_, _F_));
SymbolicBayesTreeOrdered bayesTree;
// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_;
SymbolicBayesTree::insert(bayesTree, A);
SymbolicBayesTree::insert(bayesTree, B);
SymbolicBayesTree::insert(bayesTree, C);
SymbolicBayesTree::insert(bayesTree, D);
SymbolicBayesTree::insert(bayesTree, E);
SymbolicBayesTree::insert(bayesTree, F);
SymbolicBayesTree::insert(bayesTree, G);
SymbolicBayesTreeOrdered::insert(bayesTree, A);
SymbolicBayesTreeOrdered::insert(bayesTree, B);
SymbolicBayesTreeOrdered::insert(bayesTree, C);
SymbolicBayesTreeOrdered::insert(bayesTree, D);
SymbolicBayesTreeOrdered::insert(bayesTree, E);
SymbolicBayesTreeOrdered::insert(bayesTree, F);
SymbolicBayesTreeOrdered::insert(bayesTree, G);
//bayesTree.print("BayesTree");
//bayesTree.print("BayesTreeOrdered");
//bayesTree.saveGraph("BT1.dot");
SymbolicBayesTree::sharedClique rootClique= bayesTree.root();
SymbolicBayesTreeOrdered::sharedClique rootClique= bayesTree.root();
//rootClique->printTree();
SymbolicBayesTree::Cliques allCliques;
SymbolicBayesTreeOrdered::Cliques allCliques;
getAllCliques(rootClique,allCliques);
BayesNet<IndexConditional> bn;
BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) {
BayesNetOrdered<IndexConditionalOrdered> bn;
BOOST_FOREACH(SymbolicBayesTreeOrdered::sharedClique& clique, allCliques) {
//clique->print("Clique#");
bn = clique->shortcut(rootClique, &EliminateSymbolic);
//bn.print("Shortcut:\n");
@ -294,13 +294,13 @@ TEST( BayesTree, shortcutCheck )
// Check if all the cached shortcuts are cleared
rootClique->deleteCachedShortcuts();
BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) {
BOOST_FOREACH(SymbolicBayesTreeOrdered::sharedClique& clique, allCliques) {
bool notCleared = clique->cachedSeparatorMarginal();
CHECK( notCleared == false);
}
EXPECT_LONGS_EQUAL(0, rootClique->numCachedSeparatorMarginals());
// BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) {
// BOOST_FOREACH(SymbolicBayesTreeOrdered::sharedClique& clique, allCliques) {
// clique->print("Clique#");
// if(clique->cachedShortcut()){
// 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
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
BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans;
BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTreeOrdered::Cliques orphans;
list<Index> keys; keys += _B_,_S_;
bayesTree.removeTop(keys, bn, orphans);
SymbolicFactorGraph factors(bn);
SymbolicFactorGraphOrdered factors(bn);
// Check expected outcome
SymbolicFactorGraph expected;
SymbolicFactorGraphOrdered expected;
expected.push_factor(_E_,_L_,_B_);
// expected.push_factor(_L_,_B_);
// expected.push_factor(_B_);
expected.push_factor(_S_,_L_,_B_);
CHECK(assert_equal(expected, factors));
SymbolicBayesTree::Cliques expectedOrphans;
SymbolicBayesTreeOrdered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_T_], bayesTree[_X_];
CHECK(assert_equal(expectedOrphans, orphans));
// Try removeTop again with a factor that should not change a thing
boost::shared_ptr<IndexFactor> newFactor2(new IndexFactor(_B_));
BayesNet<IndexConditional> bn2;
SymbolicBayesTree::Cliques orphans2;
boost::shared_ptr<IndexFactorOrdered> newFactor2(new IndexFactorOrdered(_B_));
BayesNetOrdered<IndexConditionalOrdered> bn2;
SymbolicBayesTreeOrdered::Cliques orphans2;
keys.clear(); keys += _B_;
bayesTree.removeTop(keys, bn2, orphans2);
SymbolicFactorGraph factors2(bn2);
SymbolicFactorGraph expected2;
SymbolicFactorGraphOrdered factors2(bn2);
SymbolicFactorGraphOrdered expected2;
CHECK(assert_equal(expected2, factors2));
SymbolicBayesTree::Cliques expectedOrphans2;
SymbolicBayesTreeOrdered::Cliques expectedOrphans2;
CHECK(assert_equal(expectedOrphans2, orphans2));
}
/* ************************************************************************* */
TEST( BayesTree, removeTop2 )
TEST( BayesTreeOrdered, removeTop2 )
{
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree();
// create two factors to be inserted
SymbolicFactorGraph newFactors;
SymbolicFactorGraphOrdered newFactors;
newFactors.push_factor(_B_);
newFactors.push_factor(_S_);
// Remove the contaminated part of the Bayes tree
BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans;
BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTreeOrdered::Cliques orphans;
list<Index> keys; keys += _B_,_S_;
bayesTree.removeTop(keys, bn, orphans);
SymbolicFactorGraph factors(bn);
SymbolicFactorGraphOrdered factors(bn);
// Check expected outcome
SymbolicFactorGraph expected;
SymbolicFactorGraphOrdered expected;
expected.push_factor(_E_,_L_,_B_);
// expected.push_factor(_L_,_B_);
// expected.push_factor(_B_);
expected.push_factor(_S_,_L_,_B_);
CHECK(assert_equal(expected, factors));
SymbolicBayesTree::Cliques expectedOrphans;
SymbolicBayesTreeOrdered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_T_], bayesTree[_X_];
CHECK(assert_equal(expectedOrphans, orphans));
}
/* ************************************************************************* */
TEST( BayesTree, removeTop3 )
TEST( BayesTreeOrdered, removeTop3 )
{
const Index _x4_=5, _l5_=6;
// simple test case that failed after COLAMD was fixed/activated
IndexConditional::shared_ptr
X(new IndexConditional(_l5_)),
A(new IndexConditional(_x4_, _l5_)),
B(new IndexConditional(_x2_, _x4_)),
C(new IndexConditional(_x3_, _x2_));
IndexConditionalOrdered::shared_ptr
X(new IndexConditionalOrdered(_l5_)),
A(new IndexConditionalOrdered(_x4_, _l5_)),
B(new IndexConditionalOrdered(_x2_, _x4_)),
C(new IndexConditionalOrdered(_x3_, _x2_));
// Ordering newOrdering;
// newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_;
SymbolicBayesTree bayesTree;
SymbolicBayesTree::insert(bayesTree, X);
SymbolicBayesTree::insert(bayesTree, A);
SymbolicBayesTree::insert(bayesTree, B);
SymbolicBayesTree::insert(bayesTree, C);
SymbolicBayesTreeOrdered bayesTree;
SymbolicBayesTreeOrdered::insert(bayesTree, X);
SymbolicBayesTreeOrdered::insert(bayesTree, A);
SymbolicBayesTreeOrdered::insert(bayesTree, B);
SymbolicBayesTreeOrdered::insert(bayesTree, C);
// remove all
list<Index> keys;
keys += _l5_, _x2_, _x3_, _x4_;
BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans;
BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTreeOrdered::Cliques orphans;
bayesTree.removeTop(keys, bn, orphans);
SymbolicFactorGraph factors(bn);
SymbolicFactorGraphOrdered factors(bn);
CHECK(orphans.size() == 0);
}
/* ************************************************************************* */
TEST( BayesTree, permute )
TEST( BayesTreeOrdered, permute )
{
// 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);
// Build a bayes tree around reduced keys as if just eliminated from subset of factors/variables
IndexConditional::shared_ptr
A(new IndexConditional(_A_)),
B(new IndexConditional(_B_, _A_)),
C(new IndexConditional(_C_, _A_)),
D(new IndexConditional(_D_, _C_)),
E(new IndexConditional(_E_, _B_)),
F(new IndexConditional(_F_, _E_));
SymbolicBayesTree bayesTreeReduced;
SymbolicBayesTree::insert(bayesTreeReduced, A);
SymbolicBayesTree::insert(bayesTreeReduced, B);
SymbolicBayesTree::insert(bayesTreeReduced, C);
SymbolicBayesTree::insert(bayesTreeReduced, D);
SymbolicBayesTree::insert(bayesTreeReduced, E);
SymbolicBayesTree::insert(bayesTreeReduced, F);
IndexConditionalOrdered::shared_ptr
A(new IndexConditionalOrdered(_A_)),
B(new IndexConditionalOrdered(_B_, _A_)),
C(new IndexConditionalOrdered(_C_, _A_)),
D(new IndexConditionalOrdered(_D_, _C_)),
E(new IndexConditionalOrdered(_E_, _B_)),
F(new IndexConditionalOrdered(_F_, _E_));
SymbolicBayesTreeOrdered bayesTreeReduced;
SymbolicBayesTreeOrdered::insert(bayesTreeReduced, A);
SymbolicBayesTreeOrdered::insert(bayesTreeReduced, B);
SymbolicBayesTreeOrdered::insert(bayesTreeReduced, C);
SymbolicBayesTreeOrdered::insert(bayesTreeReduced, D);
SymbolicBayesTreeOrdered::insert(bayesTreeReduced, E);
SymbolicBayesTreeOrdered::insert(bayesTreeReduced, F);
// bayesTreeReduced.print("Reduced bayes tree");
// P( 4 5)
@ -460,7 +460,7 @@ TEST( BayesTree, permute )
// P( 0 | 1)
// Apply the permutation - should add placeholders for variables not present in nodes
SymbolicBayesTree actBayesTree = *bayesTreeReduced.clone();
SymbolicBayesTreeOrdered actBayesTree = *bayesTreeReduced.clone();
actBayesTree.permuteWithInverse(expReducingPermutation);
// actBayesTree.print("Full bayes tree");
@ -472,14 +472,14 @@ TEST( BayesTree, permute )
// check keys in cliques
std::vector<Index> expRootIndices; expRootIndices += _B0_, _A0_;
IndexConditional::shared_ptr
expRoot(new IndexConditional(expRootIndices, 2)), // root
A0(new IndexConditional(_A0_)),
B0(new IndexConditional(_B0_, _A0_)),
C0(new IndexConditional(_C0_, _A0_)), // leaf level 1
D0(new IndexConditional(_D0_, _C0_)), // leaf level 2
E0(new IndexConditional(_E0_, _B0_)), // leaf level 2
F0(new IndexConditional(_F0_, _E0_)); // leaf level 3
IndexConditionalOrdered::shared_ptr
expRoot(new IndexConditionalOrdered(expRootIndices, 2)), // root
A0(new IndexConditionalOrdered(_A0_)),
B0(new IndexConditionalOrdered(_B0_, _A0_)),
C0(new IndexConditionalOrdered(_C0_, _A0_)), // leaf level 1
D0(new IndexConditionalOrdered(_D0_, _C0_)), // leaf level 2
E0(new IndexConditionalOrdered(_E0_, _B0_)), // leaf level 2
F0(new IndexConditionalOrdered(_F0_, _E0_)); // leaf level 3
CHECK(actBayesTree.root());
EXPECT(assert_equal(*expRoot, *actBayesTree.root()->conditional()));
@ -491,13 +491,13 @@ TEST( BayesTree, permute )
// check nodes structure
LONGS_EQUAL(9, actBayesTree.nodes().size());
SymbolicBayesTree expFullTree;
SymbolicBayesTree::insert(expFullTree, A0);
SymbolicBayesTree::insert(expFullTree, B0);
SymbolicBayesTree::insert(expFullTree, C0);
SymbolicBayesTree::insert(expFullTree, D0);
SymbolicBayesTree::insert(expFullTree, E0);
SymbolicBayesTree::insert(expFullTree, F0);
SymbolicBayesTreeOrdered expFullTree;
SymbolicBayesTreeOrdered::insert(expFullTree, A0);
SymbolicBayesTreeOrdered::insert(expFullTree, B0);
SymbolicBayesTreeOrdered::insert(expFullTree, C0);
SymbolicBayesTreeOrdered::insert(expFullTree, D0);
SymbolicBayesTreeOrdered::insert(expFullTree, E0);
SymbolicBayesTreeOrdered::insert(expFullTree, F0);
EXPECT(assert_equal(expFullTree, actBayesTree));
}
@ -508,11 +508,11 @@ TEST( BayesTree, permute )
// * | / \ |
// * x1 / \ x6
// */
//TEST( BayesTree, insert )
//TEST( BayesTreeOrdered, insert )
//{
// // 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;
// SymbolicFactorGraph fg1, fg2, fg3;
// SymbolicFactorGraphOrdered fg1, fg2, fg3;
// fg1.push_factor(_x3_, _x4_);
// fg2.push_factor(_x1_, _x2_);
// fg2.push_factor(_x2_, _x3_);
@ -525,16 +525,16 @@ TEST( BayesTree, permute )
//// Ordering ordering2; ordering2 += _x1_, _x2_;
//// Ordering ordering3; ordering3 += _x6_, _x5_;
//
// BayesNet<IndexConditional> bn1, bn2, bn3;
// BayesNetOrdered<IndexConditionalOrdered> bn1, bn2, bn3;
// bn1 = *SymbolicSequentialSolver::EliminateUntil(fg1, _x4_+1);
// bn2 = *SymbolicSequentialSolver::EliminateUntil(fg2, _x2_+1);
// bn3 = *SymbolicSequentialSolver::EliminateUntil(fg3, _x5_+1);
//
// // insert child cliques
// SymbolicBayesTree actual;
// list<SymbolicBayesTree::sharedClique> children;
// SymbolicBayesTree::sharedClique r1 = actual.insert(bn2, children);
// SymbolicBayesTree::sharedClique r2 = actual.insert(bn3, children);
// SymbolicBayesTreeOrdered actual;
// list<SymbolicBayesTreeOrdered::sharedClique> children;
// SymbolicBayesTreeOrdered::sharedClique r1 = actual.insert(bn2, children);
// SymbolicBayesTreeOrdered::sharedClique r2 = actual.insert(bn3, children);
//
// // insert root clique
// children.push_back(r1);
@ -542,7 +542,7 @@ TEST( BayesTree, permute )
// actual.insert(bn1, children, true);
//
// // traditional way
// SymbolicFactorGraph fg;
// SymbolicFactorGraphOrdered fg;
// fg.push_factor(_x3_, _x4_);
// fg.push_factor(_x1_, _x2_);
// fg.push_factor(_x2_, _x3_);
@ -552,8 +552,8 @@ TEST( BayesTree, permute )
// fg.push_factor(_x6_, _x4_);
//
//// Ordering ordering; ordering += _x1_, _x2_, _x6_, _x5_, _x3_, _x4_;
// BayesNet<IndexConditional> bn(*SymbolicSequentialSolver(fg).eliminate());
// SymbolicBayesTree expected(bn);
// BayesNetOrdered<IndexConditionalOrdered> bn(*SymbolicSequentialSolver(fg).eliminate());
// SymbolicBayesTreeOrdered expected(bn);
// 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 <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/ClusterTree.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/ClusterTreeOrdered.h>
using namespace gtsam;
// explicit instantiation and typedef
namespace gtsam { template class ClusterTree<SymbolicFactorGraph>; }
typedef ClusterTree<SymbolicFactorGraph> SymbolicClusterTree;
namespace gtsam { template class ClusterTreeOrdered<SymbolicFactorGraphOrdered>; }
typedef ClusterTreeOrdered<SymbolicFactorGraphOrdered> SymbolicClusterTree;
/* ************************************************************************* */
TEST(ClusterTree, constructor) {
TEST(ClusterTreeOrdered, constructor) {
SymbolicClusterTree tree;
}

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

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

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

@ -19,33 +19,33 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/inference/SymbolicSequentialSolver.h>
#include <gtsam/inference/EliminationTreeOrdered-inl.h>
#include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
using namespace gtsam;
using namespace std;
class EliminationTreeTester {
class EliminationTreeOrderedTester {
public:
// 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[1]);
SymbolicEliminationTree::shared_ptr node1(new SymbolicEliminationTree(1));
SymbolicEliminationTreeOrdered::shared_ptr node1(new SymbolicEliminationTreeOrdered(1));
node1->add(fg[2]);
node1->add(leaf0);
SymbolicEliminationTree::shared_ptr node2(new SymbolicEliminationTree(2));
SymbolicEliminationTreeOrdered::shared_ptr node2(new SymbolicEliminationTreeOrdered(2));
node2->add(fg[3]);
node2->add(node1);
SymbolicEliminationTree::shared_ptr leaf3(new SymbolicEliminationTree(3));
SymbolicEliminationTreeOrdered::shared_ptr leaf3(new SymbolicEliminationTreeOrdered(3));
leaf3->add(fg[4]);
SymbolicEliminationTree::shared_ptr etree(new SymbolicEliminationTree(4));
SymbolicEliminationTreeOrdered::shared_ptr etree(new SymbolicEliminationTreeOrdered(4));
etree->add(leaf3);
etree->add(node2);
@ -53,20 +53,20 @@ public:
}
};
TEST(EliminationTree, Create)
TEST(EliminationTreeOrdered, Create)
{
// create example factor graph
SymbolicFactorGraph fg;
SymbolicFactorGraphOrdered fg;
fg.push_factor(0, 1);
fg.push_factor(0, 2);
fg.push_factor(1, 4);
fg.push_factor(2, 4);
fg.push_factor(3, 4);
SymbolicEliminationTree::shared_ptr expected = EliminationTreeTester::buildHardcodedTree(fg);
SymbolicEliminationTreeOrdered::shared_ptr expected = EliminationTreeOrderedTester::buildHardcodedTree(fg);
// Build from factor graph
SymbolicEliminationTree::shared_ptr actual = SymbolicEliminationTree::Create(fg);
SymbolicEliminationTreeOrdered::shared_ptr actual = SymbolicEliminationTreeOrdered::Create(fg);
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)
/* ************************************************************************* */
TEST(EliminationTree, eliminate )
TEST(EliminationTreeOrdered, eliminate )
{
// create expected Chordal bayes Net
SymbolicBayesNet expected;
expected.push_front(boost::make_shared<IndexConditional>(4));
expected.push_front(boost::make_shared<IndexConditional>(3,4));
expected.push_front(boost::make_shared<IndexConditional>(2,4));
expected.push_front(boost::make_shared<IndexConditional>(1,2,4));
expected.push_front(boost::make_shared<IndexConditional>(0,1,2));
SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditionalOrdered>(4));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(3,4));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(2,4));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(1,2,4));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(0,1,2));
// Create factor graph
SymbolicFactorGraph fg;
SymbolicFactorGraphOrdered fg;
fg.push_factor(0, 1);
fg.push_factor(0, 2);
fg.push_factor(1, 4);
@ -95,20 +95,20 @@ TEST(EliminationTree, eliminate )
fg.push_factor(3, 4);
// eliminate
SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate();
SymbolicBayesNetOrdered actual = *SymbolicSequentialSolver(fg).eliminate();
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST(EliminationTree, disconnected_graph) {
SymbolicFactorGraph fg;
TEST(EliminationTreeOrdered, disconnected_graph) {
SymbolicFactorGraphOrdered fg;
fg.push_factor(0, 1);
fg.push_factor(0, 2);
fg.push_factor(1, 2);
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 <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST(FactorGraph, eliminateFrontals) {
TEST(FactorGraphOrdered, eliminateFrontals) {
SymbolicFactorGraph sfgOrig;
SymbolicFactorGraphOrdered sfgOrig;
sfgOrig.push_factor(0,1);
sfgOrig.push_factor(0,2);
sfgOrig.push_factor(1,3);
@ -43,15 +43,15 @@ TEST(FactorGraph, eliminateFrontals) {
sfgOrig.push_factor(2,3);
sfgOrig.push_factor(4,5);
IndexConditional::shared_ptr actualCond;
SymbolicFactorGraph actualSfg;
IndexConditionalOrdered::shared_ptr actualCond;
SymbolicFactorGraphOrdered actualSfg;
boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2);
vector<Index> condIndices;
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(4,5);
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 <gtsam/inference/BayesNet.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/ISAM.h>
#include <gtsam/inference/BayesNetOrdered.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/ISAMOrdered.h>
using namespace std;
using namespace gtsam;
typedef ISAM<IndexConditional> SymbolicISAM;
typedef ISAMOrdered<IndexConditionalOrdered> SymbolicISAM;
/* ************************************************************************* */
// 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 <gtsam/inference/inference.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/inferenceOrdered.h>
#include <gtsam/inference/VariableIndexOrdered.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
using namespace gtsam;
/* ************************************************************************* */
TEST(inference, UnobservedVariables) {
SymbolicFactorGraph sfg;
SymbolicFactorGraphOrdered sfg;
// Create a factor graph that skips some variables
sfg.push_factor(0,1);
sfg.push_factor(1,3);
sfg.push_factor(3,5);
VariableIndex variableIndex(sfg);
VariableIndexOrdered variableIndex(sfg);
// Computes a permutation with known variables first, skipped variables last
// Actual 0 1 3 5 2 4
@ -50,7 +50,7 @@ TEST(inference, UnobservedVariables) {
/* ************************************************************************* */
TEST(inference, constrained_ordering) {
SymbolicFactorGraph sfg;
SymbolicFactorGraphOrdered sfg;
// create graph with wanted variable set = 2, 4
sfg.push_factor(0,1);
@ -59,7 +59,7 @@ TEST(inference, constrained_ordering) {
sfg.push_factor(3,4);
sfg.push_factor(4,5);
VariableIndex variableIndex(sfg);
VariableIndexOrdered variableIndex(sfg);
// unconstrained version
Permutation::shared_ptr actUnconstrained(inference::PermutationCOLAMD(variableIndex));
@ -83,7 +83,7 @@ TEST(inference, constrained_ordering) {
/* ************************************************************************* */
TEST(inference, grouped_constrained_ordering) {
SymbolicFactorGraph sfg;
SymbolicFactorGraphOrdered sfg;
// create graph with constrained groups:
// 1: 2, 4
@ -94,7 +94,7 @@ TEST(inference, grouped_constrained_ordering) {
sfg.push_factor(3,4);
sfg.push_factor(4,5);
VariableIndex variableIndex(sfg);
VariableIndexOrdered variableIndex(sfg);
// constrained version - push one set to the end
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 <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/inference/ClusterTree.h>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/SymbolicSequentialSolver.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/JunctionTreeOrdered.h>
#include <gtsam/inference/ClusterTreeOrdered.h>
#include <gtsam/inference/JunctionTreeOrdered.h>
#include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
using namespace gtsam;
using namespace std;
typedef JunctionTree<SymbolicFactorGraph> SymbolicJunctionTree;
typedef JunctionTreeOrdered<SymbolicFactorGraphOrdered> SymbolicJunctionTree;
/* ************************************************************************* *
* x1 - x2 - x3 - x4
* x3 x4
* x2 x1 : x3
****************************************************************************/
TEST( JunctionTree, constructor )
TEST( JunctionTreeOrdered, constructor )
{
const Index x2=0, x1=1, x3=2, x4=3;
SymbolicFactorGraph fg;
SymbolicFactorGraphOrdered fg;
fg.push_factor(x2,x1);
fg.push_factor(x2,x3);
fg.push_factor(x3,x4);
@ -71,19 +71,19 @@ TEST( JunctionTree, constructor )
* x3 x4
* x2 x1 : x3
****************************************************************************/
TEST( JunctionTree, eliminate)
TEST( JunctionTreeOrdered, eliminate)
{
const Index x2=0, x1=1, x3=2, x4=3;
SymbolicFactorGraph fg;
SymbolicFactorGraphOrdered fg;
fg.push_factor(x2,x1);
fg.push_factor(x2,x3);
fg.push_factor(x3,x4);
SymbolicJunctionTree jt(fg);
SymbolicBayesTree::sharedClique actual = jt.eliminate(&EliminateSymbolic);
SymbolicBayesTreeOrdered::sharedClique actual = jt.eliminate(&EliminateSymbolic);
BayesNet<IndexConditional> bn(*SymbolicSequentialSolver(fg).eliminate());
SymbolicBayesTree expected(bn);
BayesNetOrdered<IndexConditionalOrdered> bn(*SymbolicSequentialSolver(fg).eliminate());
SymbolicBayesTreeOrdered expected(bn);
// cout << "BT from JT:\n";
// actual->printTree("");

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

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

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

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

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