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

add check in GaussianMixtureFactor::likelihood

release/4.3a0
Varun Agrawal 2023-01-04 02:53:12 -05:00
parent bb31956a96
commit ed16c335c1
1 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/hybrid/GaussianMixture.cpp
View File

@ -69,8 +69,7 @@ GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const {
GaussianFactorGraph result;
result.push_back(conditional);
if (conditional) {
return GraphAndConstant(
result, conditional->logNormalizationConstant());
return GraphAndConstant(result, conditional->logNormalizationConstant());
} else {
return GraphAndConstant(result, 0.0);
}
@ -163,7 +162,13 @@ KeyVector GaussianMixture::continuousParents() const {
/* ************************************************************************* */
boost::shared_ptr<GaussianMixtureFactor> GaussianMixture::likelihood(
const VectorValues &frontals) const {
// TODO(dellaert): check that values has all frontals
// Check that values has all frontals
for (auto &&kv : frontals) {
if (frontals.find(kv.first) == frontals.end()) {
throw std::runtime_error("GaussianMixture: frontals missing factor key.");
}
}
const DiscreteKeys discreteParentKeys = discreteKeys();
const KeyVector continuousParentKeys = continuousParents();
const GaussianMixtureFactor::Factors likelihoods(