Skip to content
Snippets Groups Projects
Commit 23e2b7b7 authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Improve check() [Cherry picked] 

Cherry picked from adapting-to-multiple-processor-motion-285
parent 1261502a
No related branches found
No related tags found
1 merge request!340Resolve "Problem::check() more exhaustive"
This commit is part of merge request !340. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
Showing
with 89 additions and 22 deletions
src/problem/problem.cpp
+ 89
− 22
View file @ 23e2b7b7
......@@ -1694,7 +1694,7 @@ bool Problem::check(int verbose_level) const
if ( Fo ) // case FRAME:
{
if (verbose_level > 0)
cout << " --> Frm" << Fo->id() << " <- ";
cout << " ( --> Frm" << Fo->id() << " <- ";
bool found = false;
for (auto cby : Fo->getConstrainedByList())
{
......@@ -1703,7 +1703,7 @@ bool Problem::check(int verbose_level) const
found = found || (c == cby);
}
if (verbose_level > 0)
cout << endl;
cout << ")";
// check constrained_by pointer in constrained frame
// is_consistent = is_consistent && found;
......@@ -1719,7 +1719,7 @@ bool Problem::check(int verbose_level) const
if ( Co ) // case CAPTURE:
{
if (verbose_level > 0)
cout << " --> Cap" << Co->id() << " <- ";
cout << " ( --> Cap" << Co->id() << " <- ";
bool found = false;
for (auto cby : Co->getConstrainedByList())
{
......@@ -1728,7 +1728,7 @@ bool Problem::check(int verbose_level) const
found = found || (c == cby);
}
if (verbose_level > 0)
cout << endl;
cout << ")";
// check constrained_by pointer in constrained frame
// is_consistent = is_consistent && found;
......@@ -1744,7 +1744,7 @@ bool Problem::check(int verbose_level) const
if ( fo ) // case FEATURE:
{
if (verbose_level > 0)
cout << " --> Ftr" << fo->id() << " <- ";
cout << " ( --> Ftr" << fo->id() << " <- ";
bool found = false;
for (auto cby : fo->getConstrainedByList())
{
......@@ -1753,7 +1753,7 @@ bool Problem::check(int verbose_level) const
found = found || (c == cby);
}
if (verbose_level > 0)
cout << endl;
cout << ")";
// check constrained_by pointer in constrained feature
// is_consistent = is_consistent && found;
......@@ -1768,7 +1768,7 @@ bool Problem::check(int verbose_level) const
if ( Lo ) // case LANDMARK:
{
if (verbose_level > 0)
cout << " --> Lmk" << Lo->id() << " <- ";
cout << " ( --> Lmk" << Lo->id() << " <- ";
bool found = false;
for (auto cby : Lo->getConstrainedByList())
{
......@@ -1777,7 +1777,7 @@ bool Problem::check(int verbose_level) const
found = found || (c == cby);
}
if (verbose_level > 0)
cout << endl;
cout << ")";
// check constrained_by pointer in constrained landmark
// is_consistent = is_consistent && found;
......@@ -1787,6 +1787,9 @@ bool Problem::check(int verbose_level) const
//Clear inconsistency_explanation
std::stringstream().swap(inconsistency_explanation);
}
if (verbose_level > 0)
cout << endl;
if (verbose_level > 0)
{
cout << " -> Prb @ " << c->getProblem().get() << endl;
......@@ -1824,38 +1827,102 @@ bool Problem::check(int verbose_level) const
cout << " (lp @ " << lp.get() << ")";
}
}
bool found_here;
std::vector<StateBlockPtr> sb_vec;
// find in own Frame
found = found || (std::find_if(F->getStateBlockMap().begin(), F->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != F->getStateBlockMap().end());
sb_vec = F->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " F" << F->id();
found = found || found_here;
// find in own Capture
found = found || (std::find(C->getStateBlockVec().begin(), C->getStateBlockVec().end(), sb) != C->getStateBlockVec().end());
sb_vec = C->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " C" << C->id();
found = found || found_here;
// Find in other Captures of the own Frame
if (!found_here)
for (auto FC : F->getCaptureList())
{
sb_vec = FC->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " F" << F->id() << ".C" << FC->id();
found = found || found_here;
}
// find in own Sensor
if (S)
{
auto sb_vec = S->getStateBlockVec();
found = found || (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
sb_vec = S->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " S" << S->id();
found = found || found_here;
}
// find in constrained Frame
if (Fo)
found = found || (std::find_if(Fo->getStateBlockMap().begin(), Fo->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != Fo->getStateBlockMap().end());
if (Fo){
sb_vec = Fo->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " Fo" << Fo->id();
found = found || found_here;
}
// find in constrained Capture
if (Co)
found = found || (std::find(Co->getStateBlockVec().begin(), Co->getStateBlockVec().end(), sb) != Co->getStateBlockVec().end());
{
sb_vec = Co->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " Co" << Co->id();
found = found || found_here;
}
// Find in other Captures of the constrained Frame
if (!found_here && Fo)
for (auto FoC : Fo->getCaptureList())
{
sb_vec = FoC->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " Fo" << Fo->id() << ".C" << FoC->id();
found = found || found_here;
}
// find in constrained Feature
if (fo)
{
// find in constrained feature's Frame
FrameBasePtr foF = fo->getFrame();
found = found || (std::find_if(foF->getStateBlockMap().begin(), foF->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != foF->getStateBlockMap().end());
auto foF = fo->getFrame();
sb_vec = foF->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " foF" << foF->id();
found = found || found_here;
// find in constrained feature's Capture
CaptureBasePtr foC = fo->getCapture();
found = found || (std::find(foC->getStateBlockVec().begin(), foC->getStateBlockVec().end(), sb) != foC->getStateBlockVec().end());
auto foC = fo->getCapture();
sb_vec = foC->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " foC" << foC->id();
found = found || found_here;
// find in constrained feature's Sensor
SensorBasePtr foS = fo->getCapture()->getSensor();
found = found || (std::find(foS->getStateBlockVec().begin(), foS->getStateBlockVec().end(), sb) != foS->getStateBlockVec().end());
auto foS = fo->getCapture()->getSensor();
sb_vec = foS->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " foS" << foS->id();
found = found || found_here;
}
// find in constrained landmark
if (Lo)
found = found || (std::find_if(Lo->getStateBlockMap().begin(), Lo->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != Lo->getStateBlockMap().end());
{
sb_vec = Lo->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " Lo" << Lo->id();
found = found || found_here;
}
if (verbose_level > 0)
{
if (found)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment