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

42
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,18 +55,22 @@ 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";
factors_.print( if (factors_.empty()) {
"", [&](Key k) { return formatter(k); }, std::cout << " empty" << std::endl;
[&](const GaussianFactor::shared_ptr &gf) -> std::string { } else {
RedirectCout rd; factors_.print(
std::cout << ":\n"; "", [&](Key k) { return formatter(k); },
if (gf && !gf->empty()) { [&](const GaussianFactor::shared_ptr &gf) -> std::string {
gf->print("", formatter); RedirectCout rd;
return rd.str(); std::cout << ":\n";
} else { if (gf && !gf->empty()) {
return "nullptr"; gf->print("", formatter);
} return rd.str();
}); } else {
return "nullptr";
}
});
}
std::cout << "}" << std::endl; std::cout << "}" << std::endl;
} }