diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 002a4aaf95d3c227e49d7e3f99a50a7f6f648a8c..76c90a4569bd9585c09b92fc89e8ace80e3b11c2 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -1873,75 +1873,94 @@ bool Problem::check(int verbose_level) const std::vector<StateBlockPtr> sb_vec; // find in own Frame - sb_vec = F->getStateBlockVec(); - found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.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; + found = found || found_here; // find in own Capture - sb_vec = C->getStateBlockVec(); - found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.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; + 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 << " FC" << FC->id(); + found = found || found_here; + } // find in own Sensor if (S) { - sb_vec = S->getStateBlockVec(); - found_here = (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; + found = found || found_here; } // find in constrained Frame if (Fo){ - sb_vec = Fo->getStateBlockVec(); - found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end()); + 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; + found = found || found_here; + + // Find in Captures of the constrained Frame + 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 << " FoC" << FoC->id(); + found = found || found_here; + } } // find in constrained Capture if (Co) { - sb_vec = Co->getStateBlockVec(); - found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.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; + found = found || found_here; } // find in constrained Feature if (fo) { // find in constrained feature's Frame - FrameBasePtr foF = fo->getFrame(); - sb_vec = foF->getStateBlockVec(); - found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.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; + found = found || found_here; // find in constrained feature's Capture - CaptureBasePtr foC = fo->getCapture(); - sb_vec = foC->getStateBlockVec(); - found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.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; + found = found || found_here; // find in constrained feature's Sensor - SensorBasePtr foS = fo->getCapture()->getSensor(); - sb_vec = foS->getStateBlockVec(); - found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.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; + found = found || found_here; } // find in constrained landmark if (Lo) { - sb_vec = Lo->getStateBlockVec(); - found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.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; + found = found || found_here; } if (verbose_level > 0)