From 2dce5d4c4fec9cdea13e5c24e068261a325bdb87 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:11:01 +0100
Subject: [PATCH] Improve check()
Show where the factor's SB's where found
---
src/problem/problem.cpp | 65 ++++++++++++++++++++++++++++++++++-------
1 file changed, 54 insertions(+), 11 deletions(-)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 855236ece..b6eb30d82 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)
--
GitLab