Skip to content
Snippets Groups Projects
Commit 2dce5d4c authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Improve check()

Show where the factor's SB's where found
parent 2768132d
No related branches found
No related tags found
No related merge requests found
......@@ -1690,38 +1690,81 @@ 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 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 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());
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());
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());
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% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment