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

24
gtsam/hybrid/GaussianMixture.cpp
View File

@ -88,16 +88,20 @@ 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 =
if (Cgm_Kgcm > 0.0) { this->logConstant_ - gc->logNormalizationConstant();
// We add a constant factor which will be used when computing // If there is a difference in the covariances, we need to account for
// the probability of the discrete variables. // that since the error is dependent on the mode.
Vector c(1); if (Cgm_Kgcm > 0.0) {
c << std::sqrt(2.0 * Cgm_Kgcm); // We add a constant factor which will be used when computing
auto constantFactor = std::make_shared<JacobianFactor>(c); // the probability of the discrete variables.
return GaussianFactorGraph{gc, constantFactor}; Vector c(1);
c << std::sqrt(2.0 * Cgm_Kgcm);
auto constantFactor = std::make_shared<JacobianFactor>(c);
return GaussianFactorGraph{gc, constantFactor};
}
} }
return GaussianFactorGraph{gc}; return GaussianFactorGraph{gc};
}; };