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

Made the code a bit more readable

release/4.3a0
Frank Dellaert 2013-12-19 17:13:38 +00:00
parent 197a3efef9
commit f93b6829cc
1 changed files with 31 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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