From 6cd773c3a6f33a7cdcac43e02f8510b1713cfdee Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sun, 29 Mar 2020 00:28:58 +0100
Subject: [PATCH] Improve check()
---
src/problem/problem.cpp | 79 +++++++++++++++++++++++++----------------
1 file changed, 49 insertions(+), 30 deletions(-)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 002a4aaf9..76c90a456 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)
--
GitLab