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

leverage hiding inside HybridGaussianFactor

release/4.3a0
Varun Agrawal 2024-09-15 09:30:59 -04:00
parent 6b9fb5b22f
commit 1b7435361e
1 changed files with 3 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/hybrid/HybridGaussianConditional.cpp
View File

@ -229,15 +229,10 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
if (Cgm_Kgcm == 0.0) { if (Cgm_Kgcm == 0.0) {
return {likelihood_m, 0.0}; return {likelihood_m, 0.0};
} else { } else {
// Add a constant factor to the likelihood in case the noise models // Add a constant to the likelihood in case the noise models
// are not all equal. // are not all equal.
GaussianFactorGraph gfg; double c = 2.0 * Cgm_Kgcm;
gfg.push_back(likelihood_m); return {likelihood_m, c};
Vector c(1);
c << std::sqrt(2.0 * Cgm_Kgcm);
auto constantFactor = std::make_shared<JacobianFactor>(c);
gfg.push_back(constantFactor);
return {std::make_shared<JacobianFactor>(gfg), 0.0};
} }
}); });
return std::make_shared<HybridGaussianFactor>( return std::make_shared<HybridGaussianFactor>(