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

add optional ordering and fix bug

release/4.3a0
Varun Agrawal 2022-11-08 13:36:18 -05:00
parent 64cd7c938f
commit 3987b036a7
2 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -563,7 +563,7 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::continuousProbPrimes(
/* ************************************************************************ */ /* ************************************************************************ */
boost::shared_ptr<HybridGaussianFactorGraph::BayesNetType> boost::shared_ptr<HybridGaussianFactorGraph::BayesNetType>
HybridGaussianFactorGraph::eliminateHybridSequential() const { HybridGaussianFactorGraph::eliminateHybridSequential(const boost::optional<Ordering> continuous, const boost::optional<Ordering> discrete) const {
Ordering continuous_ordering(this->continuousKeys()), Ordering continuous_ordering(this->continuousKeys()),
discrete_ordering(this->discreteKeys()); discrete_ordering(this->discreteKeys());
@ -577,6 +577,11 @@ HybridGaussianFactorGraph::eliminateHybridSequential() const {
auto last_conditional = bayesNet->at(bayesNet->size() - 1); auto last_conditional = bayesNet->at(bayesNet->size() - 1);
DiscreteKeys discrete_keys = last_conditional->discreteKeys(); DiscreteKeys discrete_keys = last_conditional->discreteKeys();
// If not discrete variables, return the eliminated bayes net.
if (discrete_keys.size() == 0) {
return bayesNet;
}
const std::vector<DiscreteValues> assignments = const std::vector<DiscreteValues> assignments =
DiscreteValues::CartesianProduct(discrete_keys); DiscreteValues::CartesianProduct(discrete_keys);

6
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -227,9 +227,13 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
* @brief Custom elimination function which computes the correct * @brief Custom elimination function which computes the correct
* continuous probabilities. * continuous probabilities.
* *
* @param continuous Optional ordering for all continuous variables.
* @param discrete Optional ordering for all discrete variables.
* @return boost::shared_ptr<BayesNetType> * @return boost::shared_ptr<BayesNetType>
*/ */
boost::shared_ptr<BayesNetType> eliminateHybridSequential() const; boost::shared_ptr<BayesNetType> eliminateHybridSequential(
const boost::optional<Ordering> continuous,
const boost::optional<Ordering> discrete) const;
/** /**
* @brief Return a Colamd constrained ordering where the discrete keys are * @brief Return a Colamd constrained ordering where the discrete keys are