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

check for valid GaussianConditional

release/4.3a0
Varun Agrawal 2024-09-03 17:36:45 -04:00
parent 06ecf00dba
commit 3f782a4ae7
1 changed files with 14 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/hybrid/GaussianMixture.cpp
View File

@ -88,9 +88,12 @@ GaussianFactorGraphTree GaussianMixture::add(
/* *******************************************************************************/ /* *******************************************************************************/
GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const { GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const {
auto wrap = [this](const GaussianConditional::shared_ptr &gc) { auto wrap = [this](const GaussianConditional::shared_ptr &gc) {
const double Cgm_Kgcm = this->logConstant_ - gc->logNormalizationConstant(); // First check if conditional has not been pruned
// If there is a difference in the covariances, we need to account for that if (gc) {
// since the error is dependent on the mode. const double Cgm_Kgcm =
this->logConstant_ - gc->logNormalizationConstant();
// If there is a difference in the covariances, we need to account for
// that since the error is dependent on the mode.
if (Cgm_Kgcm > 0.0) { if (Cgm_Kgcm > 0.0) {
// We add a constant factor which will be used when computing // We add a constant factor which will be used when computing
// the probability of the discrete variables. // the probability of the discrete variables.
@ -99,6 +102,7 @@ GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const {
auto constantFactor = std::make_shared<JacobianFactor>(c); auto constantFactor = std::make_shared<JacobianFactor>(c);
return GaussianFactorGraph{gc, constantFactor}; return GaussianFactorGraph{gc, constantFactor};
} }
}
return GaussianFactorGraph{gc}; return GaussianFactorGraph{gc};
}; };
return {conditionals_, wrap}; return {conditionals_, wrap};