diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 17fe5d3482619a62f2cf37123f3e50e371c366ac..8ba9bd9acab0ef571d8859b361b369428ddeba4f 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -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)