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

remove deferredFactors

release/4.3a0
Varun Agrawal 2023-01-04 02:54:34 -05:00
parent ed16c335c1
commit 34daecd7a4
1 changed files with 8 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -60,10 +60,10 @@ template class EliminateableFactorGraph<HybridGaussianFactorGraph>;
/* ************************************************************************ */ /* ************************************************************************ */
static GaussianFactorGraphTree addGaussian( static GaussianFactorGraphTree addGaussian(
const GaussianFactorGraphTree &sum, const GaussianFactorGraphTree &gfgTree,
const GaussianFactor::shared_ptr &factor) { const GaussianFactor::shared_ptr &factor) {
// If the decision tree is not initialized, then initialize it. // If the decision tree is not initialized, then initialize it.
if (sum.empty()) { if (gfgTree.empty()) {
GaussianFactorGraph result; GaussianFactorGraph result;
result.push_back(factor); result.push_back(factor);
return GaussianFactorGraphTree(GraphAndConstant(result, 0.0)); return GaussianFactorGraphTree(GraphAndConstant(result, 0.0));
@ -74,20 +74,18 @@ static GaussianFactorGraphTree addGaussian(
result.push_back(factor); result.push_back(factor);
return GraphAndConstant(result, graph_z.constant); return GraphAndConstant(result, graph_z.constant);
}; };
return sum.apply(add); return gfgTree.apply(add);
} }
} }
/* ************************************************************************ */ /* ************************************************************************ */
// TODO(dellaert): We need to document why deferredFactors need to be // TODO(dellaert): Implementation-wise, it's probably more efficient to first
// added last, which I would undo if possible. Implementation-wise, it's // collect the discrete keys, and then loop over all assignments to populate a
// probably more efficient to first collect the discrete keys, and then loop // vector.
// over all assignments to populate a vector.
GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
gttic(assembleGraphTree); gttic(assembleGraphTree);
GaussianFactorGraphTree result; GaussianFactorGraphTree result;
std::vector<GaussianFactor::shared_ptr> deferredFactors;
for (auto &f : factors_) { for (auto &f : factors_) {
// TODO(dellaert): just use a virtual method defined in HybridFactor. // TODO(dellaert): just use a virtual method defined in HybridFactor.
@ -101,10 +99,10 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
} else if (f->isContinuous()) { } else if (f->isContinuous()) {
if (auto gf = boost::dynamic_pointer_cast<HybridGaussianFactor>(f)) { if (auto gf = boost::dynamic_pointer_cast<HybridGaussianFactor>(f)) {
deferredFactors.push_back(gf->inner()); result = addGaussian(result, gf->inner());
} }
if (auto cg = boost::dynamic_pointer_cast<HybridConditional>(f)) { if (auto cg = boost::dynamic_pointer_cast<HybridConditional>(f)) {
deferredFactors.push_back(cg->asGaussian()); result = addGaussian(result, cg->asGaussian());
} }
} else if (f->isDiscrete()) { } else if (f->isDiscrete()) {
@ -126,10 +124,6 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
} }
} }
for (auto &f : deferredFactors) {
result = addGaussian(result, f);
}
gttoc(assembleGraphTree); gttoc(assembleGraphTree);
return result; return result;