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

Merge pull request #1852 from borglab/fix-continuousKeys

release/4.3a0
Varun Agrawal 2024-09-28 20:26:15 -04:00 committed by GitHub
parent 8816374bdd 8904f5f3ff
commit 7a9300c9c1
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
4 changed files with 16 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/hybrid/HybridFactorGraph.cpp
View File

@ -18,6 +18,7 @@
*/
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
@ -58,6 +59,8 @@ const KeySet HybridFactorGraph::continuousKeySet() const {
}
} else if (auto p = std::dynamic_pointer_cast<GaussianFactor>(factor)) {
keys.insert(p->keys().begin(), p->keys().end());
} else if (auto p = std::dynamic_pointer_cast<NonlinearFactor>(factor)) {
keys.insert(p->keys().begin(), p->keys().end());
}
}
return keys;

9
gtsam/hybrid/HybridGaussianConditional.cpp
View File

@ -69,12 +69,14 @@ HybridGaussianConditional::HybridGaussianConditional(
conditionals_(conditionals),
negLogConstant_(helper.minNegLogConstant) {}
/* *******************************************************************************/
HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKeys &discreteParents,
const HybridGaussianConditional::Conditionals &conditionals)
: HybridGaussianConditional(discreteParents, conditionals,
ConstructorHelper(conditionals)) {}
/* *******************************************************************************/
HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKey &discreteParent,
const std::vector<GaussianConditional::shared_ptr> &conditionals)
@ -242,13 +244,6 @@ std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &discreteKeys) {
}
/* ************************************************************************* */
/**
* @brief Helper function to get the pruner functional.
*
* @param discreteProbs The probabilities of only discrete keys.
* @return std::function<GaussianConditional::shared_ptr(
* const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
*/
std::function<GaussianConditional::shared_ptr(
const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
HybridGaussianConditional::prunerFunc(const DecisionTreeFactor &discreteProbs) {

8
gtsam/hybrid/HybridGaussianConditional.h
View File

@ -194,7 +194,13 @@ class GTSAM_EXPORT HybridGaussianConditional
/// Convert to a DecisionTree of Gaussian factor graphs.
GaussianFactorGraphTree asGaussianFactorGraphTree() const;
//// Get the pruner functor from pruned discrete probabilities.
/**
* @brief Get the pruner function from discrete probabilities.
*
* @param discreteProbs The probabilities of only discrete keys.
* @return std::function<GaussianConditional::shared_ptr(
* const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
*/
std::function<GaussianConditional::shared_ptr(
const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
prunerFunc(const DecisionTreeFactor &prunedProbabilities);

4
gtsam/hybrid/HybridGaussianFactor.cpp
View File

@ -126,16 +126,19 @@ HybridGaussianFactor::HybridGaussianFactor(const ConstructorHelper &helper)
factors_(helper.factorsTree.empty() ? augment(helper.pairs)
: helper.factorsTree) {}
/* *******************************************************************************/
HybridGaussianFactor::HybridGaussianFactor(
const DiscreteKey &discreteKey,
const std::vector<GaussianFactor::shared_ptr> &factors)
: HybridGaussianFactor(ConstructorHelper(discreteKey, factors)) {}
/* *******************************************************************************/
HybridGaussianFactor::HybridGaussianFactor(
const DiscreteKey &discreteKey,
const std::vector<GaussianFactorValuePair> &factorPairs)
: HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {}
/* *******************************************************************************/
HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys &discreteKeys,
const FactorValuePairs &factors)
: HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {}
@ -223,6 +226,7 @@ double HybridGaussianFactor::potentiallyPrunedComponentError(
return std::numeric_limits<double>::max();
}
}
/* *******************************************************************************/
AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
const VectorValues &continuousValues) const {