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

Added error for all versions - should become logDiensity?

release/4.3a0
Frank Dellaert 2023-01-02 09:46:08 -05:00
parent 7c270618dd
commit bd8d2ea2c1
2 changed files with 21 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/hybrid/HybridConditional.cpp
View File

@ -17,6 +17,7 @@
#include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/inference/Conditional-inst.h>
#include <gtsam/inference/Key.h>
@ -107,15 +108,30 @@ bool HybridConditional::equals(const HybridFactor &other, double tol) const {
auto other = e->asMixture();
return other != nullptr && gm->equals(*other, tol);
}
if (auto gm = asGaussian()) {
if (auto gc = asGaussian()) {
auto other = e->asGaussian();
return other != nullptr && gm->equals(*other, tol);
return other != nullptr && gc->equals(*other, tol);
}
if (auto gm = asDiscrete()) {
if (auto dc = asDiscrete()) {
auto other = e->asDiscrete();
return other != nullptr && gm->equals(*other, tol);
return other != nullptr && dc->equals(*other, tol);
}
return inner_->equals(*(e->inner_), tol);
}
/* ************************************************************************ */
double HybridConditional::error(const HybridValues &values) const {
if (auto gm = asMixture()) {
return gm->error(values);
}
if (auto gc = asGaussian()) {
return gc->error(values.continuous());
}
if (auto dc = asDiscrete()) {
return -log((*dc)(values.discrete()));
}
throw std::runtime_error(
"HybridConditional::error: conditional type not handled");
}
} // namespace gtsam

10
gtsam/hybrid/HybridConditional.h
View File

@ -176,15 +176,7 @@ class GTSAM_EXPORT HybridConditional
boost::shared_ptr<Factor> inner() const { return inner_; }
/// Return the error of the underlying conditional.
/// Currently only implemented for Gaussian mixture.
double error(const HybridValues& values) const override {
if (auto gm = asMixture()) {
return gm->error(values);
} else {
throw std::runtime_error(
"HybridConditional::error: only implemented for Gaussian mixture");
}
}
double error(const HybridValues& values) const override;
/// @}