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

account for extra error when sigmas are different

release/4.3a0
Varun Agrawal 2024-09-03 15:17:18 -04:00
parent d34dd659bc
commit 82fc9b9eeb
1 changed files with 12 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/hybrid/GaussianMixture.cpp
View File

@ -87,7 +87,18 @@ GaussianFactorGraphTree GaussianMixture::add(
/* *******************************************************************************/
GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const {
auto wrap = [](const GaussianConditional::shared_ptr &gc) {
auto wrap = [this](const GaussianConditional::shared_ptr &gc) {
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) {
// We add a constant factor which will be used when computing
// the probability of the discrete variables.
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 {conditionals_, wrap};