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

clean up HybridGaussianFactorGraph

release/4.3a0
Varun Agrawal 2023-01-03 01:50:16 -05:00
parent 38f3209d97
commit 195dddfdeb
1 changed files with 9 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -174,7 +174,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
} }
} }
// TODO(dellaert): This does sum-product. For max-product, use EliminateForMPE // NOTE: This does sum-product. For max-product, use EliminateForMPE.
auto result = EliminateDiscrete(dfg, frontalKeys); auto result = EliminateDiscrete(dfg, frontalKeys);
return {boost::make_shared<HybridConditional>(result.first), return {boost::make_shared<HybridConditional>(result.first),
@ -194,6 +194,7 @@ GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) {
}; };
return GaussianFactorGraphTree(sum, emptyGaussian); return GaussianFactorGraphTree(sum, emptyGaussian);
} }
/* ************************************************************************ */ /* ************************************************************************ */
static std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr> static std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
hybridElimination(const HybridGaussianFactorGraph &factors, hybridElimination(const HybridGaussianFactorGraph &factors,
@ -209,7 +210,9 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
// decision tree indexed by all discrete keys involved. // decision tree indexed by all discrete keys involved.
GaussianFactorGraphTree sum = factors.assembleGraphTree(); GaussianFactorGraphTree sum = factors.assembleGraphTree();
// TODO(dellaert): does assembleGraphTree not guarantee this? // Convert factor graphs with a nullptr to an empty factor graph.
// This is done after assembly since it is non-trivial to keep track of which
// FG has a nullptr as we're looping over the factors.
sum = removeEmpty(sum); sum = removeEmpty(sum);
using EliminationPair = std::pair<boost::shared_ptr<GaussianConditional>, using EliminationPair = std::pair<boost::shared_ptr<GaussianConditional>,
@ -230,7 +233,8 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
boost::tie(conditional, newFactor) = boost::tie(conditional, newFactor) =
EliminatePreferCholesky(graph_z.graph, frontalKeys); EliminatePreferCholesky(graph_z.graph, frontalKeys);
// Get the log of the log normalization constant inverse. // Get the log of the log normalization constant inverse and
// add it to the previous constant.
const double logZ = const double logZ =
graph_z.constant - conditional->logNormalizationConstant(); graph_z.constant - conditional->logNormalizationConstant();
// Get the log of the log normalization constant inverse. // Get the log of the log normalization constant inverse.
@ -273,13 +277,9 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
auto factorProb = auto factorProb =
[&](const GaussianMixtureFactor::FactorAndConstant &factor_z) { [&](const GaussianMixtureFactor::FactorAndConstant &factor_z) {
// This is the probability q(μ) at the MLE point. // This is the probability q(μ) at the MLE point.
// factor_z.factor is a factor without keys, just containing the // factor_z.factor is a factor without keys,
// residual. // just containing the residual.
return exp(-factor_z.error(VectorValues())); return exp(-factor_z.error(VectorValues()));
// TODO(dellaert): this is not correct, since VectorValues() is not
// the MLE point. But it does not matter, as at the MLE point the
// error will be zero, hence:
// return exp(factor_z.constant);
}; };
const DecisionTree<Key, double> fdt(newFactors, factorProb); const DecisionTree<Key, double> fdt(newFactors, factorProb);
@ -301,8 +301,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
boost::make_shared<HybridDiscreteFactor>(discreteFactor)}; boost::make_shared<HybridDiscreteFactor>(discreteFactor)};
} else { } else {
// Create a resulting GaussianMixtureFactor on the separator. // Create a resulting GaussianMixtureFactor on the separator.
// TODO(dellaert): Keys can be computed from the factors, so we should not
// need to pass them in.
return {boost::make_shared<HybridConditional>(gaussianMixture), return {boost::make_shared<HybridConditional>(gaussianMixture),
boost::make_shared<GaussianMixtureFactor>( boost::make_shared<GaussianMixtureFactor>(
continuousSeparator, discreteSeparator, newFactors)}; continuousSeparator, discreteSeparator, newFactors)};