diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 855236ece07f90a1570d293ac5c9333ee445658c..b6eb30d8200be000e513c84c931dfe3a331f9646 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -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)