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

if checks for dynamic_cast

release/4.3a0
Varun Agrawal 2022-06-02 22:45:12 -04:00
parent 2afa6781f8
commit 3e10514846
1 changed files with 16 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -82,23 +82,19 @@ continuousElimination(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys) {
GaussianFactorGraph gfg;
for (auto &fp : factors) {
auto ptr = boost::dynamic_pointer_cast<HybridGaussianFactor>(fp);
if (ptr) {
if (auto ptr = boost::dynamic_pointer_cast<HybridGaussianFactor>(fp)) {
gfg.push_back(ptr->inner());
} else if (auto p =
boost::static_pointer_cast<HybridConditional>(fp)->inner()) {
gfg.push_back(boost::static_pointer_cast<GaussianConditional>(p));
} else {
auto p = boost::static_pointer_cast<HybridConditional>(fp)->inner();
if (p) {
gfg.push_back(boost::static_pointer_cast<GaussianConditional>(p));
} else {
// It is an orphan wrapped conditional
}
// It is an orphan wrapped conditional
}
}
auto result = EliminatePreferCholesky(gfg, frontalKeys);
return std::make_pair(
boost::make_shared<HybridConditional>(result.first),
boost::make_shared<HybridGaussianFactor>(result.second));
return {boost::make_shared<HybridConditional>(result.first),
boost::make_shared<HybridGaussianFactor>(result.second)};
}
/* ************************************************************************ */
@ -107,24 +103,20 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys) {
DiscreteFactorGraph dfg;
for (auto &fp : factors) {
auto ptr = boost::dynamic_pointer_cast<HybridDiscreteFactor>(fp);
if (ptr) {
if (auto ptr = boost::dynamic_pointer_cast<HybridDiscreteFactor>(fp)) {
dfg.push_back(ptr->inner());
} else if (auto p =
boost::static_pointer_cast<HybridConditional>(fp)->inner()) {
dfg.push_back(boost::static_pointer_cast<DiscreteConditional>(p));
} else {
auto p = boost::static_pointer_cast<HybridConditional>(fp)->inner();
if (p) {
dfg.push_back(boost::static_pointer_cast<DiscreteConditional>(p));
} else {
// It is an orphan wrapper
}
// It is an orphan wrapper
}
}
auto result = EliminateDiscrete(dfg, frontalKeys);
return std::make_pair(
boost::make_shared<HybridConditional>(result.first),
boost::make_shared<HybridDiscreteFactor>(result.second));
return {boost::make_shared<HybridConditional>(result.first),
boost::make_shared<HybridDiscreteFactor>(result.second)};
}
/* ************************************************************************ */
@ -146,13 +138,11 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
for (auto &f : factors) {
if (f->isHybrid()) {
auto cgmf = boost::dynamic_pointer_cast<GaussianMixtureFactor>(f);
if (cgmf) {
if (auto cgmf = boost::dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
sum = cgmf->add(sum);
}
auto gm = boost::dynamic_pointer_cast<HybridConditional>(f);
if (gm) {
if (auto gm = boost::dynamic_pointer_cast<HybridConditional>(f)) {
sum = gm->asMixture()->add(sum);
}