diff --git a/include/core/utils/check_log.hpp b/include/core/utils/check_log.hpp
index db3ab2dc9dd6e165c3541544bf7770022568f2fe..b1905d7b7c1a0afdb8247ee8686467a104b2b330 100644
--- a/include/core/utils/check_log.hpp
+++ b/include/core/utils/check_log.hpp
@@ -2,31 +2,42 @@
#define CHECK_LOG_HPP
#include <iostream>
#include <string>
+#include <sstream>
-class CheckLog {
+namespace wolf
+{
+class CheckLog
+{
+ public:
+ bool is_consistent_;
+ std::string explanation_;
-public:
-
- bool is_consistent_;
- std::string explanation_;
-
- CheckLog() {
- is_consistent_ = true;
- explanation_ = "";
- }
- CheckLog(bool consistent, std::string explanation) {
- is_consistent_ = consistent;
- if (not consistent)
- explanation_ = explanation;
- else
- explanation_ = "";
- }
- ~CheckLog(){};
- void compose(CheckLog l) {
-
- CheckLog result_log;
- is_consistent_ = is_consistent_ and l.is_consistent_;
- explanation_ = explanation_ + l.explanation_;
- }
+ CheckLog()
+ {
+ is_consistent_ = true;
+ explanation_ = "";
+ }
+ CheckLog(bool consistent, std::string explanation)
+ {
+ is_consistent_ = consistent;
+ if (not consistent)
+ explanation_ = explanation;
+ else
+ explanation_ = "";
+ }
+ ~CheckLog(){};
+ void compose(CheckLog l)
+ {
+ is_consistent_ = is_consistent_ and l.is_consistent_;
+ explanation_ = explanation_ + l.explanation_;
+ }
+ void addAssertion(bool condition, std::stringstream& stream)
+ {
+ auto cl = CheckLog(condition, stream.str());
+ this->compose(cl);
+ // Clear inconsistency_explanation
+ std::stringstream().swap(stream);
+ }
};
+} // namespace wolf
#endif
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index b60d915f411d1ded2a19df2de2ee18b9bdc0850c..dc3b90ef9abcc7e7a7818f8e2dbda5f0985c5134 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -1153,12 +1153,6 @@ void Problem::print(int depth, std::ostream& stream, bool constr_by, bool metric
bool Problem::check(bool verbose, std::ostream& stream) const
{
- //////////////////////////////////////////
- // TODO remove these variables and usings:
- using std::cout;
- using std::endl;
- bool is_consistent;
- //////////////////////////////////////////
CheckLog log(true, "");
log.explanation_ = "## WOLF::problem inconsistencies list \n ---------------------------------- \n";
@@ -1183,9 +1177,8 @@ bool Problem::check(bool verbose, std::ostream& stream) const
std::stringstream inconsistency_explanation;
inconsistency_explanation << "Hardware problem pointer is " << H->getProblem().get()
<< " but problem pointer is " << P_raw << "\n";
- log.compose(CheckLog((H->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ log.addAssertion((H->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
// Sensors =======================================================================================
for (auto S : H->getSensorList())
@@ -1212,15 +1205,13 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// check problem and hardware pointers
inconsistency_explanation << "Sensor problem pointer is " << S->getProblem().get()
<< " but problem pointer is " << P_raw << "\n";
- log.compose(CheckLog((S->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ log.addAssertion((S->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
inconsistency_explanation << "Sensor hardware pointer is " << S->getHardware()
<< " but hardware pointer is " << H << "\n";
- log.compose(CheckLog((S->getHardware() == H), inconsistency_explanation.str()));
+ log.addAssertion((S->getHardware() == H), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
// Processors =======================================================================================
for (auto p : S->getProcessorList())
@@ -1235,15 +1226,13 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// check problem and sensor pointers
inconsistency_explanation << "Processor problem pointer is " << p->getProblem().get()
<< " but problem pointer is " << P_raw << "\n";
- log.compose(CheckLog((p->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ log.addAssertion((p->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
inconsistency_explanation << "Processor sensor pointer is " << p->getSensor()
<< " but sensor pointer is " << P_raw << "\n";
- log.compose(CheckLog((p->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ log.addAssertion((p->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
}
}
// ------------------------
@@ -1258,9 +1247,8 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// check pointer to Problem
inconsistency_explanation << "Trajectory problem pointer is " << T->getProblem().get()
<< " but problem pointer is" << P_raw << "\n";
- log.compose(CheckLog((T->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ log.addAssertion((T->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
// Frames =======================================================================================
for (auto F : T->getFrameList())
@@ -1276,12 +1264,10 @@ bool Problem::check(bool verbose, std::ostream& stream) const
auto sb = pair.second;
// check for valid state block
- inconsistency_explanation << "Frame's " << F.get()
+ inconsistency_explanation << "Frame " << F->id() << " @ "<< F.get()
<< " has State block pointer " << sb.get()
- << " null! \n";
- log.compose(CheckLog((sb.get() != 0), inconsistency_explanation.str()));
- // Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " = 0 \n";
+ log.addAssertion((sb.get() != 0), inconsistency_explanation);
if (verbose) {
stream << " "<< pair.first << " sb @ " << sb.get();
@@ -1297,34 +1283,28 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// check problem pointer
inconsistency_explanation << "Frame problem pointer is " << F->getProblem().get()
<< " but problem pointer is" << P_raw << "\n";
- log.compose(CheckLog((F->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
- inconsistency_explanation.clear();
+ log.addAssertion((F->getProblem().get() == P_raw), inconsistency_explanation);
// check trajectory pointer
inconsistency_explanation << "Frame trajectory pointer is " << F->getTrajectory()
<< " but trajectory pointer is" << T << "\n";
- log.compose(CheckLog((F->getTrajectory() == T), inconsistency_explanation.str()));
+ log.addAssertion((F->getTrajectory() == T), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
// check constrained_by
for (auto cby : F->getConstrainedByList())
{
if (verbose)
{
- cout << " <- c" << cby->id() << " -> ";
+ stream << " <- c" << cby->id() << " -> ";
for (const auto& Fow : cby->getFrameOtherList())
- cout << " F" << Fow.lock()->id() << endl;
+ stream << " F" << Fow.lock()->id() << std::endl;
}
// check constrained_by pointer to this frame
- inconsistency_explanation << "constrained-by frame pointer " << F
+ inconsistency_explanation << "constrained-by frame " << F->id() << " @ " << F
<< " not found among constrained-by factors\n";
- log.compose(CheckLog((cby->hasFrameOther(F)), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.addAssertion((cby->hasFrameOther(F)), inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector())
{
@@ -1358,12 +1338,9 @@ bool Problem::check(bool verbose, std::ostream& stream) const
auto sb = pair.second;
// check for valid state block
- inconsistency_explanation << "Frame's " << F.get()
- << " has State block pointer " << sb.get()
- << " null! \n";
- log.compose(CheckLog((sb.get() != 0), inconsistency_explanation.str()));
- // Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ inconsistency_explanation << "Capture " << C->id() << " @ " << C.get() << " has State block pointer "
+ << sb.get() << " = 0 \n";
+ log.addAssertion((sb.get() != 0), inconsistency_explanation);
if (verbose)
{
@@ -1380,15 +1357,13 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// check problem and frame pointers
inconsistency_explanation << "Capture problem pointer is " << C->getProblem().get()
<< " but problem pointer is" << P_raw << "\n";
- log.compose(CheckLog((C->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ log.addAssertion((C->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
inconsistency_explanation << "Capture frame pointer is " << C->getFrame()
<< " but frame pointer is" << F << "\n";
- log.compose(CheckLog((C->getFrame() == F), inconsistency_explanation.str()));
+ log.addAssertion((C->getFrame() == F), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
// check contrained_by
for (const auto& cby : C->getConstrainedByList())
@@ -1398,11 +1373,14 @@ bool Problem::check(bool verbose, std::ostream& stream) const
stream << " <- c" << cby->id() << " -> ";
for (const auto& Cow : cby->getCaptureOtherList())
stream << " C" << Cow.lock()->id();
- stream << endl;
+ stream << std::endl;
}
// check constrained_by pointer to this capture
- is_consistent = is_consistent && (cby->hasCaptureOther(C));
+ inconsistency_explanation << "constrained by capture " << C->id() << " @ " << C
+ << " not found among constrained-by factors\n";
+ log.addAssertion((cby->hasCaptureOther(C)), inconsistency_explanation);
+
for (auto sb : cby->getStateBlockPtrVector())
{
if (verbose)
@@ -1414,7 +1392,7 @@ bool Problem::check(bool verbose, std::ostream& stream) const
if (lp)
stream << " (lp @ " << lp.get() << ")";
}
- stream << endl;
+ stream << std::endl;
}
}
@@ -1434,15 +1412,13 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// check problem and capture pointers
inconsistency_explanation << "Feature frame pointer is " << f->getProblem().get()
<< " but problem pointer is" << P_raw << "\n";
- log.compose(CheckLog((f->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ log.addAssertion((f->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
inconsistency_explanation << "Feature capture pointer is " << f->getCapture()
<< " but capture pointer is" << C << "\n";
- log.compose(CheckLog((f->getCapture() == C), inconsistency_explanation.str()));
+ log.addAssertion((f->getCapture() == C), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
// check contrained_by
for (auto cby : f->getConstrainedByList())
@@ -1452,15 +1428,14 @@ bool Problem::check(bool verbose, std::ostream& stream) const
stream << " <- c" << cby->id() << " -> ";
for (const auto& fow : cby->getFeatureOtherList())
stream << " f" << fow.lock()->id();
- stream << endl;
+ stream << std::endl;
}
// check constrained_by pointer to this feature
- inconsistency_explanation << "constrained by Feature pointer is " << f
+ inconsistency_explanation << "constrained by Feature " << f->id() << " @ " << f
<< " not found among constrained-by factors\n";
- log.compose(CheckLog((cby->hasFeatureOther(f)), inconsistency_explanation.str()));
+ log.addAssertion((cby->hasFeatureOther(f)), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
}
// Factors =======================================================================================
@@ -1493,15 +1468,14 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// check constrained_by pointer in constrained frame
bool found = Fo->isConstrainedBy(c);
- inconsistency_explanation << "The constrained Feature " << Fo
- << " does not close the constrained-by loop start = " << c << "\n";
- log.compose(CheckLog((found), inconsistency_explanation.str()));
+ inconsistency_explanation << "The constrained Feature " << Fo->id() << " @ " << Fo
+ << " not found among constrained-by factors\n";
+ log.addAssertion((found), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
}
}
if (verbose && !c->getFrameOtherList().empty())
- cout << ")";
+ stream << ")";
// find constrained_by pointer in constrained capture
for (const auto& Cow : c->getCaptureOtherList())
@@ -1519,15 +1493,14 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// check constrained_by pointer in constrained frame
bool found = Co->isConstrainedBy(c);
- inconsistency_explanation << "The constrained capture " << Co
- << " does not close the constrained-by loop start = " << c << "\n";
- log.compose(CheckLog((found), inconsistency_explanation.str()));
+ inconsistency_explanation << "The constrained capture " << Co->id() << " @ " << Co
+ << " not found among constrained-by factors\n";
+ log.addAssertion((found), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
}
}
if (verbose && !c->getCaptureOtherList().empty())
- cout << ")";
+ stream << ")";
// find constrained_by pointer in constrained feature
for (const auto& fow : c->getFeatureOtherList())
@@ -1535,7 +1508,6 @@ bool Problem::check(bool verbose, std::ostream& stream) const
if (!fow.expired())
{
const auto& fo = fow.lock();
-
if (verbose)
{
stream << " ( --> f" << fo->id() << " <- ";
@@ -1545,15 +1517,13 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// check constrained_by pointer in constrained feature
bool found = fo->isConstrainedBy(c);
- inconsistency_explanation << "The constrained feature" << fo
- << " does not close the constrained-by loop start = " << c << "\n";
- log.compose(CheckLog((found), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ inconsistency_explanation << "The constrained feature " << fo->id() << " @ " << fo
+ << " not found among constrained-by factors\n";
+ log.addAssertion((found), inconsistency_explanation);
}
}
if (verbose && !c->getFeatureOtherList().empty())
- cout << ")";
+ stream << ")";
// find constrained_by pointer in constrained landmark
for (const auto& Low : c->getLandmarkOtherList())
@@ -1571,18 +1541,17 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// check constrained_by pointer in constrained landmark
bool found = Lo->isConstrainedBy(c);
- inconsistency_explanation << "The constrained landmark " << Lo
- << " does not close the constrained-by loop start = " << c << "\n";
- log.compose(CheckLog((found), inconsistency_explanation.str()));
+ inconsistency_explanation << "The constrained landmark " << Lo->id() << " @ " << Lo
+ << " not found among constrained-by factors\n";
+ log.addAssertion((found), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
}
}
if (verbose && !c->getLandmarkOtherList().empty())
stream << ")";
if (verbose)
- stream << endl;
+ stream << std::endl;
if (verbose)
{
@@ -1591,17 +1560,15 @@ bool Problem::check(bool verbose, std::ostream& stream) const
}
// check problem and feature pointers
- inconsistency_explanation << "The factor " << c << " has problem ptr " << c->getProblem().get()
+ inconsistency_explanation << "The factor " << c->id() << " @ " << c << " has problem ptr " << c->getProblem().get()
<< " but problem ptr is " << P_raw << "\n";
- log.compose(CheckLog((c->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ log.addAssertion((c->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
- inconsistency_explanation << "The factor " << c << " has feature ptr " << c->getFeature()
+ inconsistency_explanation << "The factor " << c->id() << " @ " << c << " has feature ptr " << c->getFeature()
<< " but it should have " << f << "\n";
- log.compose(CheckLog((c->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ log.addAssertion((c->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
// find state block pointers in all constrained nodes
SensorBasePtr S = c->getFeature()->getCapture()->getSensor(); // get own sensor to check sb
@@ -1623,28 +1590,28 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// find in own Frame
found_here = F->hasStateBlock(sb);
if (found_here && verbose) stream << " F" << F->id();
- found = found || found_here;
+ found = found || found_here;
// find in own Capture
found_here = C->hasStateBlock(sb);
if (found_here && verbose) stream << " 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())
- {
+ for (auto FC : F->getCaptureList())
+ {
found_here = FC->hasStateBlock(sb);
- if (found_here && verbose) stream << " F" << F->id() << ".C" << FC->id();
- found = found || found_here;
- }
+ if (found_here && verbose) stream << " F" << F->id() << ".C" << FC->id();
+ found = found || found_here;
+ }
// find in own Sensor
if (S)
{
found_here = S->hasStateBlock(sb);
if (found_here && verbose) stream << " S" << S->id();
- found = found || found_here;
+ found = found || found_here;
}
@@ -1730,14 +1697,10 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// check that the state block has been found somewhere
inconsistency_explanation << "The stateblock " << sb << " has not been found (is floating!)";
- log.compose(CheckLog((found), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
-
- inconsistency_explanation << "The stateblock " << sb << " of factor " << c << " is null\n";
- log.compose(CheckLog((sb.get() != nullptr), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.addAssertion((found), inconsistency_explanation);
+
+ inconsistency_explanation << "The stateblock " << sb << " of factor " << c->id() << " @ " << c << " is null\n";
+ log.addAssertion((sb.get() != nullptr), inconsistency_explanation);
}
}
}
@@ -1752,9 +1715,7 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// check pointer to Problem
inconsistency_explanation << "The map problem ptr is " << M->getProblem().get() << " but problem is " << P_raw << "\n";
- log.compose(CheckLog((M->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.addAssertion((M->getProblem().get() == P_raw), inconsistency_explanation);
// Landmarks =======================================================================================
for (auto L : M->getLandmarkList())
@@ -1779,20 +1740,16 @@ bool Problem::check(bool verbose, std::ostream& stream) const
}
// check problem and map pointers
- inconsistency_explanation << "The landmarks problem ptr is "
+ inconsistency_explanation << "Landmarks' problem ptr is "
<< L->getProblem().get() << " but problem is "
<< P_raw << "\n";
- log.compose(CheckLog((L->getProblem().get() == P_raw), inconsistency_explanation.str()));
- // Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.addAssertion((L->getProblem().get() == P_raw), inconsistency_explanation);
- inconsistency_explanation << "The landmarks map ptr is "
+ inconsistency_explanation << "The Landmarks' map ptr is "
<< L->getMap() << " but map is "
<< M << "\n";
- log.compose(CheckLog((M->getProblem().get() == P_raw), inconsistency_explanation.str()));
- // Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.addAssertion((M->getProblem().get() == P_raw), inconsistency_explanation);
for (auto cby : L->getConstrainedByList())
{
@@ -1808,16 +1765,13 @@ bool Problem::check(bool verbose, std::ostream& stream) const
stream << " Lmk" << Lo->id();
}
}
- stream << endl;
+ stream << std::endl;
}
// check constrained-by factors
- inconsistency_explanation << "The landmark constrained-by loop started at "
- << cby
- << " is not closed\n";
- log.compose(CheckLog((cby->hasLandmarkOther(L)), inconsistency_explanation.str()));
- // Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ inconsistency_explanation << "constrained-by landmark " << L->id() << " @ " << L
+ << " not found among constrained-by factors\n";
+ log.addAssertion((cby->hasLandmarkOther(L)), inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector()) {
if (verbose) {