Made the code a bit more readable
parent
197a3efef9
commit
f93b6829cc
|
@ -20,6 +20,7 @@
|
|||
|
||||
#include <gtsam/inference/EliminateableFactorGraph.h>
|
||||
#include <gtsam/inference/inferenceExceptions.h>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
@ -32,13 +33,15 @@ namespace gtsam {
|
|||
if(ordering && variableIndex) {
|
||||
gttic(eliminateSequential);
|
||||
// Do elimination
|
||||
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> > result
|
||||
= EliminationTreeType(asDerived(), *variableIndex, *ordering).eliminate(function);
|
||||
EliminationTreeType etree(asDerived(), *variableIndex, *ordering);
|
||||
boost::shared_ptr<BayesNetType> bayesNet;
|
||||
boost::shared_ptr<FactorGraphType> factorGraph;
|
||||
boost::tie(bayesNet,factorGraph) = etree.eliminate(function);
|
||||
// If any factors are remaining, the ordering was incomplete
|
||||
if(!result.second->empty())
|
||||
if(!factorGraph->empty())
|
||||
throw InconsistentEliminationRequested();
|
||||
// Return the Bayes net
|
||||
return result.first;
|
||||
return bayesNet;
|
||||
}
|
||||
else if(!variableIndex) {
|
||||
// If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
|
||||
|
@ -63,13 +66,16 @@ namespace gtsam {
|
|||
if(ordering && variableIndex) {
|
||||
gttic(eliminateMultifrontal);
|
||||
// Do elimination with given ordering
|
||||
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > result
|
||||
= JunctionTreeType(EliminationTreeType(asDerived(), *variableIndex, *ordering)).eliminate(function);
|
||||
EliminationTreeType etree(asDerived(), *variableIndex, *ordering);
|
||||
JunctionTreeType junctionTree(etree);
|
||||
boost::shared_ptr<BayesTreeType> bayesTree;
|
||||
boost::shared_ptr<FactorGraphType> factorGraph;
|
||||
boost::tie(bayesTree,factorGraph) = junctionTree.eliminate(function);
|
||||
// If any factors are remaining, the ordering was incomplete
|
||||
if(!result.second->empty())
|
||||
if(!factorGraph->empty())
|
||||
throw InconsistentEliminationRequested();
|
||||
// Return the Bayes tree
|
||||
return result.first;
|
||||
return bayesTree;
|
||||
}
|
||||
else if(!variableIndex) {
|
||||
// If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
|
||||
|
@ -94,7 +100,8 @@ namespace gtsam {
|
|||
if(variableIndex) {
|
||||
gttic(eliminatePartialSequential);
|
||||
// Do elimination
|
||||
return EliminationTreeType(asDerived(), *variableIndex, ordering).eliminate(function);
|
||||
EliminationTreeType etree(asDerived(), *variableIndex, ordering);
|
||||
return etree.eliminate(function);
|
||||
} else {
|
||||
// If no variable index is provided, compute one and call this function again
|
||||
return eliminatePartialSequential(ordering, function, VariableIndex(asDerived()));
|
||||
|
@ -130,7 +137,9 @@ namespace gtsam {
|
|||
if(variableIndex) {
|
||||
gttic(eliminatePartialMultifrontal);
|
||||
// Do elimination
|
||||
return JunctionTreeType(EliminationTreeType(asDerived(), *variableIndex, ordering)).eliminate(function);
|
||||
EliminationTreeType etree(asDerived(), *variableIndex, ordering);
|
||||
JunctionTreeType junctionTree(etree);
|
||||
return junctionTree.eliminate(function);
|
||||
} else {
|
||||
// If no variable index is provided, compute one and call this function again
|
||||
return eliminatePartialMultifrontal(ordering, function, VariableIndex(asDerived()));
|
||||
|
@ -172,19 +181,21 @@ namespace gtsam {
|
|||
gttic(marginalMultifrontalBayesNet);
|
||||
// An ordering was provided for the marginalized variables, so we can first eliminate them
|
||||
// in the order requested.
|
||||
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > eliminated =
|
||||
boost::shared_ptr<BayesTreeType> bayesTree;
|
||||
boost::shared_ptr<FactorGraphType> factorGraph;
|
||||
boost::tie(bayesTree,factorGraph) =
|
||||
eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex);
|
||||
|
||||
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.
|
||||
return eliminated.second->eliminateSequential(*varsAsOrdering, function);
|
||||
return factorGraph->eliminateSequential(*varsAsOrdering, function);
|
||||
}
|
||||
else
|
||||
{
|
||||
// No ordering was provided for the unmarginalized variables, so order them with COLAMD.
|
||||
return eliminated.second->eliminateSequential(boost::none, function);
|
||||
return factorGraph->eliminateSequential(boost::none, function);
|
||||
}
|
||||
}
|
||||
else
|
||||
|
@ -209,7 +220,8 @@ namespace gtsam {
|
|||
}
|
||||
} else {
|
||||
// If no variable index is provided, compute one and call this function again
|
||||
return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, VariableIndex(asDerived()));
|
||||
VariableIndex index(asDerived());
|
||||
return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -228,19 +240,21 @@ namespace gtsam {
|
|||
gttic(marginalMultifrontalBayesTree);
|
||||
// An ordering was provided for the marginalized variables, so we can first eliminate them
|
||||
// in the order requested.
|
||||
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > eliminated =
|
||||
boost::shared_ptr<BayesTreeType> bayesTree;
|
||||
boost::shared_ptr<FactorGraphType> factorGraph;
|
||||
boost::tie(bayesTree,factorGraph) =
|
||||
eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex);
|
||||
|
||||
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.
|
||||
return eliminated.second->eliminateMultifrontal(*varsAsOrdering, function);
|
||||
return factorGraph->eliminateMultifrontal(*varsAsOrdering, function);
|
||||
}
|
||||
else
|
||||
{
|
||||
// No ordering was provided for the unmarginalized variables, so order them with COLAMD.
|
||||
return eliminated.second->eliminateMultifrontal(boost::none, function);
|
||||
return factorGraph->eliminateMultifrontal(boost::none, function);
|
||||
}
|
||||
}
|
||||
else
|
||||
|
|
Loading…
Reference in New Issue