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