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

Fixed equals and print

release/4.3a0
Frank Dellaert 2022-12-29 13:12:12 -05:00
parent 1e87a81d01
commit 364417e4aa
1 changed files with 29 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/hybrid/GaussianMixtureFactor.cpp
View File

@ -35,7 +35,19 @@ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
/* *******************************************************************************/ /* *******************************************************************************/
bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
const This *e = dynamic_cast<const This *>(&lf); const This *e = dynamic_cast<const This *>(&lf);
return e != nullptr && Base::equals(*e, tol); if (e == nullptr) return false;
// This will return false if either factors_ is empty or e->factors_ is empty,
// but not if both are empty or both are not empty:
if (factors_.empty() ^ e->factors_.empty()) return false;
// Check the base and the factors:
return Base::equals(*e, tol) &&
factors_.equals(e->factors_,
[tol](const GaussianFactor::shared_ptr &f1,
const GaussianFactor::shared_ptr &f2) {
return f1->equals(*f2, tol);
});
} }
/* *******************************************************************************/ /* *******************************************************************************/
@ -43,6 +55,9 @@ void GaussianMixtureFactor::print(const std::string &s,
const KeyFormatter &formatter) const { const KeyFormatter &formatter) const {
HybridFactor::print(s, formatter); HybridFactor::print(s, formatter);
std::cout << "{\n"; std::cout << "{\n";
if (factors_.empty()) {
std::cout << " empty" << std::endl;
} else {
factors_.print( factors_.print(
"", [&](Key k) { return formatter(k); }, "", [&](Key k) { return formatter(k); },
[&](const GaussianFactor::shared_ptr &gf) -> std::string { [&](const GaussianFactor::shared_ptr &gf) -> std::string {
@ -55,6 +70,7 @@ void GaussianMixtureFactor::print(const std::string &s,
return "nullptr"; return "nullptr";
} }
}); });
}
std::cout << "}" << std::endl; std::cout << "}" << std::endl;
} }