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

remove unrequired code

release/4.3a0
Varun Agrawal 2022-12-10 09:46:26 +05:30
parent 22e4a733e0
commit 0596b2f543
4 changed files with 3 additions and 396 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
gtsam/hybrid/HybridEliminationTree.cpp
View File

@ -27,112 +27,16 @@ template class EliminationTree<HybridBayesNet, HybridGaussianFactorGraph>;
HybridEliminationTree::HybridEliminationTree( HybridEliminationTree::HybridEliminationTree(
const HybridGaussianFactorGraph& factorGraph, const HybridGaussianFactorGraph& factorGraph,
const VariableIndex& structure, const Ordering& order) const VariableIndex& structure, const Ordering& order)
: Base(factorGraph, structure, order), : Base(factorGraph, structure, order) {}
graph_(factorGraph),
variable_index_(structure) {
// Segregate the continuous and the discrete keys
std::tie(continuous_ordering_, discrete_ordering_) =
graph_.separateContinuousDiscreteOrdering(order);
}
/* ************************************************************************* */ /* ************************************************************************* */
HybridEliminationTree::HybridEliminationTree( HybridEliminationTree::HybridEliminationTree(
const HybridGaussianFactorGraph& factorGraph, const Ordering& order) const HybridGaussianFactorGraph& factorGraph, const Ordering& order)
: Base(factorGraph, order), : Base(factorGraph, order) {}
graph_(factorGraph),
variable_index_(VariableIndex(factorGraph)) {}
/* ************************************************************************* */ /* ************************************************************************* */
bool HybridEliminationTree::equals(const This& other, double tol) const { bool HybridEliminationTree::equals(const This& other, double tol) const {
return Base::equals(other, tol); return Base::equals(other, tol);
} }
/* ************************************************************************* */
std::pair<boost::shared_ptr<HybridBayesNet>,
boost::shared_ptr<HybridGaussianFactorGraph>>
HybridEliminationTree::eliminateContinuous(Eliminate function) const {
if (continuous_ordering_.size() > 0) {
This continuous_etree(graph_, variable_index_, continuous_ordering_);
return continuous_etree.Base::eliminate(function);
} else {
HybridBayesNet::shared_ptr bayesNet = boost::make_shared<HybridBayesNet>();
HybridGaussianFactorGraph::shared_ptr discreteGraph =
boost::make_shared<HybridGaussianFactorGraph>(graph_);
return std::make_pair(bayesNet, discreteGraph);
}
}
/* ************************************************************************* */
boost::shared_ptr<HybridGaussianFactorGraph>
HybridEliminationTree::addProbPrimes(
const HybridBayesNet::shared_ptr& continuousBayesNet,
const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const {
if (continuous_ordering_.size() > 0 && discrete_ordering_.size() > 0) {
// Get the last continuous conditional
// which will have all the discrete keys
HybridConditional::shared_ptr last_conditional =
continuousBayesNet->at(continuousBayesNet->size() - 1);
DiscreteKeys discrete_keys = last_conditional->discreteKeys();
// DecisionTree for P'(X|M, Z) for all mode sequences M
const AlgebraicDecisionTree<Key> probPrimeTree =
graph_.continuousProbPrimes(discrete_keys, continuousBayesNet);
// Add the model selection factor P(M|Z)
discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree));
}
return discreteGraph;
}
/* ************************************************************************* */
std::pair<boost::shared_ptr<HybridBayesNet>,
boost::shared_ptr<HybridGaussianFactorGraph>>
HybridEliminationTree::eliminateDiscrete(
Eliminate function,
const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const {
HybridBayesNet::shared_ptr discreteBayesNet;
HybridGaussianFactorGraph::shared_ptr finalGraph;
if (discrete_ordering_.size() > 0) {
This discrete_etree(*discreteGraph, VariableIndex(*discreteGraph),
discrete_ordering_);
std::tie(discreteBayesNet, finalGraph) =
discrete_etree.Base::eliminate(function);
} else {
discreteBayesNet = boost::make_shared<HybridBayesNet>();
finalGraph = discreteGraph;
}
return std::make_pair(discreteBayesNet, finalGraph);
}
/* ************************************************************************* */
std::pair<boost::shared_ptr<HybridBayesNet>,
boost::shared_ptr<HybridGaussianFactorGraph>>
HybridEliminationTree::eliminate(Eliminate function) const {
// Perform continuous elimination
HybridBayesNet::shared_ptr bayesNet;
HybridGaussianFactorGraph::shared_ptr discreteGraph;
std::tie(bayesNet, discreteGraph) = this->eliminateContinuous(function);
// If we have eliminated continuous variables
// and have discrete variables to eliminate,
// then compute P(X | M, Z)
HybridGaussianFactorGraph::shared_ptr updatedDiscreteGraph =
addProbPrimes(bayesNet, discreteGraph);
// Perform discrete elimination
HybridBayesNet::shared_ptr discreteBayesNet;
HybridGaussianFactorGraph::shared_ptr finalGraph;
std::tie(discreteBayesNet, finalGraph) =
eliminateDiscrete(function, updatedDiscreteGraph);
// Add the discrete conditionals to the hybrid conditionals
bayesNet->add(*discreteBayesNet);
return std::make_pair(bayesNet, finalGraph);
}
} // namespace gtsam } // namespace gtsam

90
gtsam/hybrid/HybridEliminationTree.h
View File

@ -26,21 +26,6 @@ namespace gtsam {
/** /**
* Elimination Tree type for Hybrid Factor Graphs. * Elimination Tree type for Hybrid Factor Graphs.
* *
* The elimination tree helps in elimination by specifying the parents to which
* the "combined factor" after elimination should be assigned to.
*
* In the case of hybrid elimination, we use the elimination tree to store the
* intermediate results.
* Concretely, we wish to estimate the unnormalized probability P'(X | M, Z) for
* each mode assignment M.
* P'(X | M, Z) can be computed by estimating X* which maximizes the continuous
* factor graph given the discrete modes, and then computing the unnormalized
* probability as P(X=X* | M, Z).
*
* This is done by eliminating the (continuous) factor graph present at each
* leaf of the discrete tree formed for the discrete sequence and using the
* inferred X* to compute the `probPrime`.
*
* @ingroup hybrid * @ingroup hybrid
*/ */
class GTSAM_EXPORT HybridEliminationTree class GTSAM_EXPORT HybridEliminationTree
@ -48,12 +33,6 @@ class GTSAM_EXPORT HybridEliminationTree
private: private:
friend class ::EliminationTreeTester; friend class ::EliminationTreeTester;
Ordering continuous_ordering_, discrete_ordering_;
/// Used to store the original factor graph to eliminate
HybridGaussianFactorGraph graph_;
/// Store the provided variable index.
VariableIndex variable_index_;
public: public:
typedef EliminationTree<HybridBayesNet, HybridGaussianFactorGraph> typedef EliminationTree<HybridBayesNet, HybridGaussianFactorGraph>
Base; ///< Base class Base; ///< Base class
@ -87,75 +66,6 @@ class GTSAM_EXPORT HybridEliminationTree
/** Test whether the tree is equal to another */ /** Test whether the tree is equal to another */
bool equals(const This& other, double tol = 1e-9) const; bool equals(const This& other, double tol = 1e-9) const;
protected:
/**
* @brief Helper method to eliminate continuous variables.
*
* If no continuous variables exist, return an empty bayes net
* and the original graph.
*
* @param function Elimination function for hybrid elimination.
* @return std::pair<boost::shared_ptr<HybridBayesNet>,
* boost::shared_ptr<HybridGaussianFactorGraph> >
*/
std::pair<boost::shared_ptr<HybridBayesNet>,
boost::shared_ptr<HybridGaussianFactorGraph>>
eliminateContinuous(Eliminate function) const;
/**
* @brief Compute the unnormalized probability P'(X | M, Z)
* for the factor graph in each leaf of the discrete tree.
* The discrete decision tree formed as a result is added to the
* `discreteGraph` for discrete elimination.
*
* @param continuousBayesNet The bayes nets corresponding to
* the eliminated continuous variables.
* @param discreteGraph Factor graph consisting of factors
* on discrete variables only.
* @return boost::shared_ptr<HybridGaussianFactorGraph>
*/
boost::shared_ptr<HybridGaussianFactorGraph> addProbPrimes(
const HybridBayesNet::shared_ptr& continuousBayesNet,
const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const;
/**
* @brief Helper method to eliminate the discrete variables after the
* continuous variables have been eliminated.
*
* If there are no discrete variables, return an empty bayes net and the
* discreteGraph which is passed in.
*
* @param function Hybrid elimination function
* @param discreteGraph The factor graph with the factor ϕ(X | M, Z).
* @return std::pair<boost::shared_ptr<HybridBayesNet>,
* boost::shared_ptr<HybridGaussianFactorGraph> >
*/
std::pair<boost::shared_ptr<HybridBayesNet>,
boost::shared_ptr<HybridGaussianFactorGraph>>
eliminateDiscrete(
Eliminate function,
const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const;
public:
/**
* @brief Override the EliminationTree eliminate method
* so we can perform hybrid elimination correctly.
*
* @param function Hybrid elimination function
* @return std::pair<boost::shared_ptr<HybridBayesNet>,
* boost::shared_ptr<HybridGaussianFactorGraph> >
*/
std::pair<boost::shared_ptr<HybridBayesNet>,
boost::shared_ptr<HybridGaussianFactorGraph>>
eliminate(Eliminate function) const;
Ordering continuousOrdering() const { return continuous_ordering_; }
Ordering discreteOrdering() const { return discrete_ordering_; }
/// Store the provided variable index.
VariableIndex variableIndex() const { return variable_index_; }
HybridGaussianFactorGraph graph() const { return graph_; }
}; };
} // namespace gtsam } // namespace gtsam

140
gtsam/hybrid/HybridJunctionTree.cpp
View File

@ -142,8 +142,7 @@ struct HybridConstructorTraversalData {
/* ************************************************************************* */ /* ************************************************************************* */
HybridJunctionTree::HybridJunctionTree( HybridJunctionTree::HybridJunctionTree(
const HybridEliminationTree& eliminationTree) const HybridEliminationTree& eliminationTree) {
: etree_(eliminationTree) {
gttic(JunctionTree_FromEliminationTree); gttic(JunctionTree_FromEliminationTree);
// Here we rely on the BayesNet having been produced by this elimination tree, // Here we rely on the BayesNet having been produced by this elimination tree,
// such that the conditionals are arranged in DFS post-order. We traverse the // such that the conditionals are arranged in DFS post-order. We traverse the
@ -170,141 +169,4 @@ HybridJunctionTree::HybridJunctionTree(
Base::remainingFactors_ = eliminationTree.remainingFactors(); Base::remainingFactors_ = eliminationTree.remainingFactors();
} }
/* ************************************************************************* */
std::pair<boost::shared_ptr<HybridBayesTree>,
boost::shared_ptr<HybridGaussianFactorGraph>>
HybridJunctionTree::eliminateContinuous(
const Eliminate& function, const HybridGaussianFactorGraph& graph,
const Ordering& continuous_ordering) const {
HybridBayesTree::shared_ptr continuousBayesTree;
HybridGaussianFactorGraph::shared_ptr discreteGraph;
if (continuous_ordering.size() > 0) {
HybridEliminationTree continuous_etree(graph, etree_.variableIndex(),
continuous_ordering);
This continuous_junction_tree(continuous_etree);
std::tie(continuousBayesTree, discreteGraph) =
continuous_junction_tree.Base::eliminate(function);
} else {
continuousBayesTree = boost::make_shared<HybridBayesTree>();
discreteGraph = boost::make_shared<HybridGaussianFactorGraph>(graph);
}
return std::make_pair(continuousBayesTree, discreteGraph);
}
/* ************************************************************************* */
std::pair<boost::shared_ptr<HybridBayesTree>,
boost::shared_ptr<HybridGaussianFactorGraph>>
HybridJunctionTree::eliminateDiscrete(
const Eliminate& function,
const HybridBayesTree::shared_ptr& continuousBayesTree,
const HybridGaussianFactorGraph::shared_ptr& discreteGraph,
const Ordering& discrete_ordering) const {
HybridBayesTree::shared_ptr updatedBayesTree;
HybridGaussianFactorGraph::shared_ptr finalGraph;
if (discrete_ordering.size() > 0) {
HybridEliminationTree discrete_etree(
*discreteGraph, VariableIndex(*discreteGraph), discrete_ordering);
This discrete_junction_tree(discrete_etree);
std::tie(updatedBayesTree, finalGraph) =
discrete_junction_tree.Base::eliminate(function);
// Get the clique with all the discrete keys.
// There should only be 1 clique.
const HybridBayesTree::sharedClique discrete_clique =
(*updatedBayesTree)[discrete_ordering.at(0)];
std::set<HybridBayesTreeClique::shared_ptr> clique_set;
for (auto node : continuousBayesTree->nodes()) {
clique_set.insert(node.second);
}
// Set the root of the bayes tree as the discrete clique
for (auto clique : clique_set) {
if (clique->conditional()->parents() ==
discrete_clique->conditional()->frontals()) {
updatedBayesTree->addClique(clique, discrete_clique);
} else {
if (clique->parent()) {
// Remove the clique from the children of the parents since it will
// get added again in addClique.
auto clique_it = std::find(clique->parent()->children.begin(),
clique->parent()->children.end(), clique);
clique->parent()->children.erase(clique_it);
updatedBayesTree->addClique(clique, clique->parent());
} else {
updatedBayesTree->addClique(clique);
}
}
}
} else {
updatedBayesTree = continuousBayesTree;
finalGraph = discreteGraph;
}
return std::make_pair(updatedBayesTree, finalGraph);
}
/* ************************************************************************* */
boost::shared_ptr<HybridGaussianFactorGraph> HybridJunctionTree::addProbPrimes(
const HybridBayesTree::shared_ptr& continuousBayesTree,
const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const {
Ordering continuous_ordering = etree_.continuousOrdering();
Ordering discrete_ordering = etree_.discreteOrdering();
// If we have eliminated continuous variables
// and have discrete variables to eliminate,
// then compute P(X | M, Z)
if (continuous_ordering.size() > 0 && discrete_ordering.size() > 0) {
// Collect all the discrete keys
DiscreteKeys discrete_keys;
for (auto node : continuousBayesTree->nodes()) {
auto node_dkeys = node.second->conditional()->discreteKeys();
discrete_keys.insert(discrete_keys.end(), node_dkeys.begin(),
node_dkeys.end());
}
// Remove duplicates and convert back to DiscreteKeys
std::set<DiscreteKey> dkeys_set(discrete_keys.begin(), discrete_keys.end());
discrete_keys = DiscreteKeys(dkeys_set.begin(), dkeys_set.end());
FactorGraphType graph = etree_.graph();
// DecisionTree for P'(X|M, Z) for all mode sequences M
const AlgebraicDecisionTree<Key> probPrimeTree =
graph.continuousProbPrimes(discrete_keys, continuousBayesTree);
// Add the model selection factor P(M|Z)
discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree));
}
return discreteGraph;
}
/* ************************************************************************* */
std::pair<HybridBayesTree::shared_ptr, HybridGaussianFactorGraph::shared_ptr>
HybridJunctionTree::eliminate(const Eliminate& function) const {
Ordering continuous_ordering = etree_.continuousOrdering();
Ordering discrete_ordering = etree_.discreteOrdering();
FactorGraphType graph = etree_.graph();
// Eliminate continuous
BayesTreeType::shared_ptr continuousBayesTree;
FactorGraphType::shared_ptr discreteGraph;
std::tie(continuousBayesTree, discreteGraph) =
this->eliminateContinuous(function, graph, continuous_ordering);
FactorGraphType::shared_ptr updatedDiscreteGraph =
this->addProbPrimes(continuousBayesTree, discreteGraph);
// Eliminate discrete variables to get the discrete bayes tree.
return this->eliminateDiscrete(function, continuousBayesTree,
updatedDiscreteGraph, discrete_ordering);
}
} // namespace gtsam } // namespace gtsam

69
gtsam/hybrid/HybridJunctionTree.h
View File

@ -51,10 +51,6 @@ class HybridEliminationTree;
*/ */
class GTSAM_EXPORT HybridJunctionTree class GTSAM_EXPORT HybridJunctionTree
: public JunctionTree<HybridBayesTree, HybridGaussianFactorGraph> { : public JunctionTree<HybridBayesTree, HybridGaussianFactorGraph> {
/// Record the elimination tree for use in hybrid elimination.
HybridEliminationTree etree_;
/// Store the provided variable index.
VariableIndex variable_index_;
public: public:
typedef JunctionTree<HybridBayesTree, HybridGaussianFactorGraph> typedef JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>
@ -72,71 +68,6 @@ class GTSAM_EXPORT HybridJunctionTree
* @return The elimination tree * @return The elimination tree
*/ */
HybridJunctionTree(const HybridEliminationTree& eliminationTree); HybridJunctionTree(const HybridEliminationTree& eliminationTree);
protected:
/**
* @brief Eliminate all the continuous variables from the factor graph.
*
* @param function The hybrid elimination function.
* @param graph The factor graph to eliminate.
* @param continuous_ordering The ordering of continuous variables.
* @return std::pair<boost::shared_ptr<HybridBayesTree>,
* boost::shared_ptr<HybridGaussianFactorGraph>>
*/
std::pair<boost::shared_ptr<HybridBayesTree>,
boost::shared_ptr<HybridGaussianFactorGraph>>
eliminateContinuous(const Eliminate& function,
const HybridGaussianFactorGraph& graph,
const Ordering& continuous_ordering) const;
/**
* @brief Eliminate all the discrete variables in the hybrid factor graph.
*
* @param function The hybrid elimination function.
* @param continuousBayesTree The bayes tree corresponding to
* the eliminated continuous variables.
* @param discreteGraph Factor graph of factors containing
* only discrete variables.
* @param discrete_ordering The elimination ordering for
* the discrete variables.
* @return std::pair<boost::shared_ptr<HybridBayesTree>,
* boost::shared_ptr<HybridGaussianFactorGraph>>
*/
std::pair<boost::shared_ptr<HybridBayesTree>,
boost::shared_ptr<HybridGaussianFactorGraph>>
eliminateDiscrete(const Eliminate& function,
const HybridBayesTree::shared_ptr& continuousBayesTree,
const HybridGaussianFactorGraph::shared_ptr& discreteGraph,
const Ordering& discrete_ordering) const;
/**
* @brief Compute the unnormalized probability P'(X | M, Z)
* for the factor graph in each leaf of the discrete tree.
* The discrete decision tree formed as a result is added to the
* `discreteGraph` for discrete elimination.
*
* @param continuousBayesTree The bayes tree corresponding to
* the eliminated continuous variables.
* @param discreteGraph Factor graph consisting of factors
* on discrete variables only.
* @return boost::shared_ptr<HybridGaussianFactorGraph>
*/
boost::shared_ptr<HybridGaussianFactorGraph> addProbPrimes(
const HybridBayesTree::shared_ptr& continuousBayesTree,
const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const;
public:
/**
* @brief Override the eliminate method so we can
* perform hybrid elimination correctly.
*
* @param function The hybrid elimination function.
* @return std::pair<boost::shared_ptr<HybridBayesTree>,
* boost::shared_ptr<HybridGaussianFactorGraph>>
*/
std::pair<boost::shared_ptr<HybridBayesTree>,
boost::shared_ptr<HybridGaussianFactorGraph>>
eliminate(const Eliminate& function) const;
}; };
} // namespace gtsam } // namespace gtsam