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

Right marginals for tiny1

release/4.3a0
Frank Dellaert 2023-01-02 13:22:16 -05:00
parent f8d75abfeb
commit 12d02bed1a
1 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -209,7 +209,7 @@ 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 we do not need this? // TODO(dellaert): does assembleGraphTree not guarantee this?
sum = removeEmpty(sum); sum = removeEmpty(sum);
using EliminationPair = std::pair<boost::shared_ptr<GaussianConditional>, using EliminationPair = std::pair<boost::shared_ptr<GaussianConditional>,
@ -234,16 +234,16 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
gttoc_(hybrid_eliminate); gttoc_(hybrid_eliminate);
#endif #endif
const double logZ = graph_z.constant - conditional->logNormalizationConstant();
// Get the log of the log normalization constant inverse. // Get the log of the log normalization constant inverse.
double logZ = -conditional->logNormalizationConstant(); // double logZ = -conditional->logNormalizationConstant();
// // IF this is the last continuous variable to eliminated, we need to
// IF this is the last continuous variable to eliminated, we need to // // calculate the error here: the value of all factors at the mean, see
// calculate the error here: the value of all factors at the mean, see // // ml_map_rao.pdf.
// ml_map_rao.pdf. // if (continuousSeparator.empty()) {
if (continuousSeparator.empty()) { // const auto posterior_mean = conditional->solve(VectorValues());
const auto posterior_mean = conditional->solve(VectorValues()); // logZ += graph_z.graph.error(posterior_mean);
logZ += graph_z.graph.error(posterior_mean); // }
}
return {conditional, {newFactor, logZ}}; return {conditional, {newFactor, logZ}};
}; };
@ -270,11 +270,12 @@ 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 residual.
// return exp(-factor_z.error(VectorValues())); // return exp(-factor_z.error(VectorValues()));
// TODO(dellaert): this is not correct, since VectorValues() is not // TODO(dellaert): this is not correct, since VectorValues() is not
// the MLE point. But it does not matter, as at the MLE point the // the MLE point. But it does not matter, as at the MLE point the
// error will be zero, hence: // error will be zero, hence:
return exp(-factor_z.constant); return exp(factor_z.constant);
}; };
const DecisionTree<Key, double> fdt(newFactors, factorProb); const DecisionTree<Key, double> fdt(newFactors, factorProb);
const auto discreteFactor = const auto discreteFactor =