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

remove debug statements and add docs

release/4.3a0
Varun Agrawal 2022-05-27 16:43:19 -04:00
parent 4ee4b37f2f
commit 9d26a3dc9d
2 changed files with 8 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
gtsam/hybrid/HybridFactorGraph.cpp
View File

@ -138,18 +138,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
KeySet continuousFrontals;
KeySet continuousSeparator;
if (DEBUG) {
std::cout << RED_BOLD << "Begin Eliminate: " << RESET;
frontalKeys.print();
}
// This initializes separatorKeys and mapFromKeyToDiscreteKey
for (auto &&factor : factors) {
if (DEBUG) {
std::cout << ">>> Adding factor: " << GREEN;
factor->print();
std::cout << RESET;
}
separatorKeys.insert(factor->begin(), factor->end());
if (!factor->isContinuous()) {
for (auto &k : factor->discreteKeys()) {
@ -183,43 +173,10 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
}
}
// Only for printing
if (DEBUG) {
std::cout << RED_BOLD << "Keys: " << RESET;
for (auto &f : frontalKeys) {
if (mapFromKeyToDiscreteKey.find(f) != mapFromKeyToDiscreteKey.end()) {
auto &key = mapFromKeyToDiscreteKey.at(f);
std::cout << boost::format(" (%1%,%2%),") %
DefaultKeyFormatter(key.first) % key.second;
} else {
std::cout << " " << DefaultKeyFormatter(f) << ",";
}
}
if (separatorKeys.size() > 0) {
std::cout << " | ";
}
for (auto &f : separatorKeys) {
if (mapFromKeyToDiscreteKey.find(f) != mapFromKeyToDiscreteKey.end()) {
auto &key = mapFromKeyToDiscreteKey.at(f);
std::cout << boost::format(" (%1%,%2%),") %
DefaultKeyFormatter(key.first) % key.second;
} else {
std::cout << DefaultKeyFormatter(f) << ",";
}
}
std::cout << "\n" << RESET;
}
// NOTE: We should really defer the product here because of pruning
// Case 1: we are only dealing with continuous
if (mapFromKeyToDiscreteKey.empty() && !allContinuousKeys.empty()) {
if (DEBUG) {
std::cout << RED_BOLD << "CONT. ONLY" << RESET << "\n";
}
GaussianFactorGraph gfg;
for (auto &fp : factors) {
auto ptr = boost::dynamic_pointer_cast<HybridGaussianFactor>(fp);
@ -231,7 +188,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
gfg.push_back(boost::static_pointer_cast<GaussianConditional>(p));
} else {
// It is an orphan wrapped conditional
if (DEBUG) std::cout << "Got an orphan conditional\n";
}
}
}
@ -244,10 +200,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
// Case 2: we are only dealing with discrete
if (allContinuousKeys.empty()) {
if (DEBUG) {
std::cout << RED_BOLD << "DISCRETE ONLY" << RESET << "\n";
}
DiscreteFactorGraph dfg;
for (auto &fp : factors) {
auto ptr = boost::dynamic_pointer_cast<HybridDiscreteFactor>(fp);
@ -259,7 +211,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
dfg.push_back(boost::static_pointer_cast<DiscreteConditional>(p));
} else {
// It is an orphan wrapper
if (DEBUG) std::cout << "Got an orphan wrapper conditional\n";
}
}
}
@ -280,10 +231,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
// sum out frontals, this is the factor on the separator
gttic(sum);
if (DEBUG) {
std::cout << RED_BOLD << "HYBRID ELIM." << RESET << "\n";
}
GaussianMixtureFactor::Sum sum;
std::vector<GaussianFactor::shared_ptr> deferredFactors;
@ -308,9 +255,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
// BayesTreeOrphanWrapper!
auto orphan = boost::dynamic_pointer_cast<
BayesTreeOrphanWrapper<HybridBayesTree::Clique>>(f);
if (orphan) {
if (DEBUG) std::cout << "Got an orphan wrapper conditional\n";
} else {
if (!orphan) {
auto &fr = *f;
throw std::invalid_argument(
std::string("factor is discrete in continuous elimination") +
@ -320,21 +265,9 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
}
for (auto &f : deferredFactors) {
if (DEBUG) {
std::cout << GREEN_BOLD << "Adding Gaussian" << RESET << "\n";
}
sum = addGaussian(sum, f);
}
if (DEBUG) {
std::cout << GREEN_BOLD << "[GFG Tree]\n" << RESET;
sum.print("", DefaultKeyFormatter, [](GaussianFactorGraph gfg) {
RedirectCout rd;
gfg.print("");
return rd.str();
});
}
gttoc(sum);
using EliminationPair = GaussianFactorGraph::EliminationResult;
@ -370,19 +303,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
auto conditional = boost::make_shared<GaussianMixtureConditional>(
frontalKeys, keysOfSeparator, discreteSeparator, conditionals);
if (DEBUG) {
std::cout << GREEN_BOLD << "[Conditional]\n" << RESET;
conditional->print();
std::cout << GREEN_BOLD << "[Separator]\n" << RESET;
separatorFactors.print("", DefaultKeyFormatter,
[](GaussianFactor::shared_ptr gc) {
RedirectCout rd;
gc->print("");
return rd.str();
});
std::cout << RED_BOLD << "[End Eliminate]\n" << RESET;
}
// If there are no more continuous parents, then we should create here a
// DiscreteFactor, with the error for each discrete choice.
if (keysOfSeparator.empty()) {

7
gtsam/hybrid/HybridGaussianFactor.h
View File

@ -43,12 +43,19 @@ class HybridGaussianFactor : public HybridFactor {
explicit HybridGaussianFactor(JacobianFactor &&jf);
public:
/// @name Testable
/// @{
/// Check equality.
virtual bool equals(const HybridFactor &lf, double tol) const override;
/// GTSAM print utility.
void print(
const std::string &s = "HybridFactor\n",
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
/// @}
GaussianFactor::shared_ptr inner() const { return inner_; }
};
} // namespace gtsam