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

remove augmentation in GaussianMixture in favor of augmentation in GaussianMixtureFactor

release/4.3a0
Varun Agrawal 2024-08-21 06:13:43 -04:00
parent 450fb0a016
commit 191fc3da11
2 changed files with 5 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/hybrid/GaussianMixture.cpp
View File

@ -200,24 +200,10 @@ std::shared_ptr<GaussianMixtureFactor> GaussianMixture::likelihood(
const GaussianMixtureFactor::Factors likelihoods( const GaussianMixtureFactor::Factors likelihoods(
conditionals_, [&](const GaussianConditional::shared_ptr &conditional) { conditionals_, [&](const GaussianConditional::shared_ptr &conditional) {
const auto likelihood_m = conditional->likelihood(given); const auto likelihood_m = conditional->likelihood(given);
const double Cgm_Kgcm =
logConstant_ - conditional->logNormalizationConstant();
if (Cgm_Kgcm == 0.0) {
return likelihood_m; return likelihood_m;
} else {
// Add a constant factor to the likelihood in case the noise models
// are not all equal.
GaussianFactorGraph gfg;
gfg.push_back(likelihood_m);
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);
}
}); });
return std::make_shared<GaussianMixtureFactor>( return std::make_shared<GaussianMixtureFactor>(
continuousParentKeys, discreteParentKeys, likelihoods); continuousParentKeys, discreteParentKeys, likelihoods, true);
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam/hybrid/GaussianMixtureFactor.cpp
View File

@ -29,7 +29,7 @@
namespace gtsam { namespace gtsam {
/** /**
* @brief Helper function to correct the [A|b] matrices in the factor components * @brief Helper function to augment the [A|b] matrices in the factor components
* with the normalizer values. * with the normalizer values.
* This is done by storing the normalizer value in * This is done by storing the normalizer value in
* the `b` vector as an additional row. * the `b` vector as an additional row.
@ -39,7 +39,7 @@ namespace gtsam {
* each component. * each component.
* @return GaussianMixtureFactor::Factors * @return GaussianMixtureFactor::Factors
*/ */
GaussianMixtureFactor::Factors correct( GaussianMixtureFactor::Factors augment(
const GaussianMixtureFactor::Factors &factors, bool varyingNormalizers) { const GaussianMixtureFactor::Factors &factors, bool varyingNormalizers) {
if (!varyingNormalizers) { if (!varyingNormalizers) {
return factors; return factors;
@ -107,7 +107,7 @@ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
const Factors &factors, const Factors &factors,
bool varyingNormalizers) bool varyingNormalizers)
: Base(continuousKeys, discreteKeys), : Base(continuousKeys, discreteKeys),
factors_(correct(factors, varyingNormalizers)) {} factors_(augment(factors, varyingNormalizers)) {}
/* *******************************************************************************/ /* *******************************************************************************/
bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {