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

use FactorAndConstant error()

release/4.3a0
Varun Agrawal 2022-12-31 12:24:58 +01:00
parent be27e3ace0
commit 99825fcfe2
1 changed files with 2 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -235,6 +235,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
gttoc_(hybrid_eliminate); gttoc_(hybrid_eliminate);
#endif #endif
//TODO(Varun) The normalizing constant has to be computed correctly
return {conditional_factor.first, {conditional_factor.second, 0.0}}; return {conditional_factor.first, {conditional_factor.second, 0.0}};
}; };
@ -257,7 +258,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
// If there are no more continuous parents, then we should create here a // If there are no more continuous parents, then we should create here a
// DiscreteFactor, with the error for each discrete choice. // DiscreteFactor, with the error for each discrete choice.
if (keysOfSeparator.empty()) { if (keysOfSeparator.empty()) {
VectorValues empty_values;
auto factorProb = auto factorProb =
[&](const GaussianMixtureFactor::FactorAndConstant &factor_z) { [&](const GaussianMixtureFactor::FactorAndConstant &factor_z) {
GaussianFactor::shared_ptr factor = factor_z.factor; GaussianFactor::shared_ptr factor = factor_z.factor;
@ -265,9 +265,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
return 0.0; // If nullptr, return 0.0 probability return 0.0; // If nullptr, return 0.0 probability
} else { } else {
// This is the probability q(μ) at the MLE point. // This is the probability q(μ) at the MLE point.
double error = double error = factor_z.error(VectorValues());
0.5 * std::abs(factor->augmentedInformation().determinant()) +
factor_z.constant;
return std::exp(-error); return std::exp(-error);
} }
}; };