diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index f9e100b49bd4d59f21041ad16ec9475c8a7b16f6..470d9fe07d56e973314cf5e18d35d40cd3007310 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -398,7 +398,8 @@ inline wolf::SizeStd Problem::getFactorNotificationMapSize() const
return factor_notification_map_.size();
}
-
} // namespace wolf
+
+
#endif // PROBLEM_H
diff --git a/include/core/utils/check_log.hpp b/include/core/utils/check_log.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..db3ab2dc9dd6e165c3541544bf7770022568f2fe
--- /dev/null
+++ b/include/core/utils/check_log.hpp
@@ -0,0 +1,32 @@
+#ifndef CHECK_LOG_HPP
+#define CHECK_LOG_HPP
+#include <iostream>
+#include <string>
+
+class CheckLog {
+
+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_;
+ }
+};
+#endif
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index eeb6fe5f67dc5ddc9f81ecee16578a2f93314a49..d3302513c4abf6b4b5b6356cb913dfa66e8422b5 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -14,6 +14,7 @@
#include "core/utils/logging.h"
#include "core/utils/params_server.hpp"
#include "core/utils/loader.hpp"
+#include "core/utils/check_log.hpp"
// IRI libs includes
@@ -185,7 +186,8 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
- const Eigen::VectorXd& _extrinsics, //
+
+ const Eigen::VectorXd& _extrinsics, //
const std::string& _intrinsics_filename)
{
@@ -1438,8 +1440,8 @@ bool Problem::check(int verbose_level) const
using std::cout;
using std::endl;
- bool is_consistent = true; // true if all checks passed; false if any check fails.
-
+ CheckLog log(true, "WOLF problem inconsistencies\n");
+
if (verbose_level) cout << endl;
if (verbose_level) cout << "Wolf tree integrity ---------------------" << endl;
auto P_raw = this;
@@ -1456,7 +1458,14 @@ bool Problem::check(int verbose_level) const
cout << "H @ " << H.get() << endl;
}
// check pointer to Problem
- is_consistent = is_consistent && (H->getProblem().get() == P_raw);
+ // is_consistent = is_consistent && (H->getProblem().get() == P_raw);
+
+ 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()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
// Sensors =======================================================================================
for (auto S : H->getSensorList())
@@ -1479,10 +1488,21 @@ bool Problem::check(int verbose_level) const
}
}
// check problem and hardware pointers
- is_consistent = is_consistent && (S->getProblem().get() == P_raw);
- is_consistent = is_consistent && (S->getHardware() == H);
-
- // Processors =======================================================================================
+ // is_consistent = is_consistent && (S->getProblem().get() == P_raw);
+ 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()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
+ // is_consistent = is_consistent && (S->getHardware() == H);
+ inconsistency_explanation << "Sensor hardware pointer is " << S->getHardware()
+ << " but hardware pointer is " << H << "\n";
+ log.compose(CheckLog((S->getHardware() == H), inconsistency_explanation.str()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
+ // Processors =======================================================================================
for (auto p : S->getProcessorList())
{
if (verbose_level > 0)
@@ -1492,8 +1512,22 @@ bool Problem::check(int verbose_level) const
cout << " -> S" << p->getSensor()->id() << " @ " << p->getSensor().get() << endl;
}
// check problem and sensor pointers
- is_consistent = is_consistent && (p->getProblem().get() == P_raw);
- is_consistent = is_consistent && (p->getSensor() == S);
+
+ // is_consistent = is_consistent && (p->getProblem().get() == P_raw);
+
+ 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()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
+ // is_consistent = is_consistent && (p->getSensor() == S);
+
+ 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()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
}
}
// ------------------------
@@ -1505,52 +1539,95 @@ bool Problem::check(int verbose_level) const
cout << "T @ " << T.get() << endl;
}
// check pointer to Problem
- is_consistent = is_consistent && (T->getProblem().get() == P_raw);
+ // is_consistent = is_consistent && (T->getProblem().get() == P_raw);
+
+
+ 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()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
// Frames =======================================================================================
for (auto F : T->getFrameList())
{
- if (verbose_level > 0)
- {
- cout << (F->isKeyOrAux() ? (F->isKey() ? " KF" : " EF") : " F") << F->id() << " @ " << F.get() << endl;
- cout << " -> P @ " << F->getProblem().get() << endl;
- cout << " -> T @ " << F->getTrajectory().get() << endl;
- for (const auto& sb : F->getStateBlockVec())
- {
- cout << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- cout << endl;
- }
+ if (verbose_level > 0) {
+ cout << (F->isKeyOrAux() ? (F->isKey() ? " KF" : " EF") : " F")
+ << F->id() << " @ " << F.get() << endl;
+ cout << " -> P @ " << F->getProblem().get() << endl;
+ cout << " -> T @ " << F->getTrajectory().get() << endl;
+ }
+ for (const auto &sb : F->getStateBlockVec()) {
+ 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);
+ if (verbose_level > 0) {
+ cout << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ cout << " (lp @ " << lp.get() << ")";
+ }
+ cout << endl;
}
+ }
+
// check problem and trajectory pointers
- is_consistent = is_consistent && (F->getProblem().get() == P_raw);
- is_consistent = is_consistent && (F->getTrajectory() == T);
+ // is_consistent = is_consistent && (F->getProblem().get() == P_raw);
+
+ 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);
+
+ // is_consistent = is_consistent && (F->getTrajectory() == T);
+
+ inconsistency_explanation << "Frame trajectory pointer is " << F->getTrajectory()
+ << " but trajectory pointer is" << T << "\n";
+ log.compose(CheckLog((F->getTrajectory() == T), inconsistency_explanation.str()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
for (auto cby : F->getConstrainedByList())
{
- if (verbose_level > 0)
- {
- cout << " <- c" << cby->id() << " -> F" << cby->getFrameOther()->id() << endl;
- }
+ for (auto sb : cby->getStateBlockPtrVector()) {
+ inconsistency_explanation
+ << "Factor " << cby.get() << " constraining " << 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);
+ }
+ if (verbose_level > 0) {
+ cout << " <- c" << cby->id() << " -> F"
+ << cby->getFrameOther()->id() << endl;
+ }
// check constrained_by pointer to this frame
- is_consistent = is_consistent && (cby->getFrameOther() == F);
+ // is_consistent = is_consistent && (cby->getFrameOther() == F);
+
+ inconsistency_explanation << "constrained-by frame pointer is " << cby->getFrameOther()
+ << " but frame pointer is" << F << "\n";
+ log.compose(CheckLog((cby->getFrameOther() == F), inconsistency_explanation.str()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
for (auto sb : cby->getStateBlockPtrVector())
{
- if (verbose_level > 0)
- {
- cout << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- cout << endl;
+
+ if (verbose_level > 0) {
+ cout << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ cout << " (lp @ " << lp.get() << ")";
}
+ cout << endl;
+ }
}
}
@@ -1565,21 +1642,37 @@ bool Problem::check(int verbose_level) const
cout << endl;
cout << " -> P @ " << C->getProblem().get() << endl;
cout << " -> F" << C->getFrame()->id() << " @ " << C->getFrame().get() << endl;
+ }
for (auto sb : C->getStateBlockVec())
{
- cout << " sb @ " << sb.get();
- if (sb)
+ if (verbose_level > 0)
{
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ cout << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ cout << " (lp @ " << lp.get() << ")";
}
cout << endl;
+ }
}
- }
+
// check problem and frame pointers
- is_consistent = is_consistent && (C->getProblem().get() == P_raw);
- is_consistent = is_consistent && (C->getFrame() == F);
+ // is_consistent = is_consistent && (C->getProblem().get() == P_raw);
+
+ 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()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
+ // is_consistent = is_consistent && (C->getFrame() == F);
+
+ inconsistency_explanation << "Capture frame pointer is " << C->getFrame()
+ << " but frame pointer is" << F << "\n";
+ log.compose(CheckLog((C->getFrame() == F), inconsistency_explanation.str()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
// Features =======================================================================================
for (auto f : C->getFeatureList())
@@ -1592,8 +1685,21 @@ bool Problem::check(int verbose_level) const
<< endl;
}
// check problem and capture pointers
- is_consistent = is_consistent && (f->getProblem().get() == P_raw);
- is_consistent = is_consistent && (f->getCapture() == C);
+ // is_consistent = is_consistent && (f->getProblem().get() == P_raw);
+
+ 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()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
+ // is_consistent = is_consistent && (f->getCapture() == C);
+
+ inconsistency_explanation << "Feature capture pointer is " << f->getCapture()
+ << " but capture pointer is" << C << "\n";
+ log.compose(CheckLog((f->getCapture() == C), inconsistency_explanation.str()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
for (auto cby : f->getConstrainedByList())
{
@@ -1602,7 +1708,13 @@ bool Problem::check(int verbose_level) const
cout << " <- c" << cby->id() << " -> f" << cby->getFeatureOther()->id() << endl;
}
// check constrained_by pointer to this feature
- is_consistent = is_consistent && (cby->getFeatureOther() == f);
+ // is_consistent = is_consistent && (cby->getFeatureOther() == f);
+
+ inconsistency_explanation << "constrained by Feature pointer is " << cby->getFeatureOther()
+ << " but feature pointer is" << f << "\n";
+ log.compose(CheckLog((cby->getFeatureOther() == f), inconsistency_explanation.str()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
}
// Factors =======================================================================================
@@ -1637,7 +1749,14 @@ bool Problem::check(int verbose_level) const
if (verbose_level > 0)
cout << endl;
// check constrained_by pointer in constrained frame
- is_consistent = is_consistent && found;
+ // is_consistent = is_consistent && found;
+
+ 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);
+
}
// find constrained_by pointer in constrained capture
@@ -1655,7 +1774,14 @@ bool Problem::check(int verbose_level) const
if (verbose_level > 0)
cout << endl;
// check constrained_by pointer in constrained frame
- is_consistent = is_consistent && found;
+ // is_consistent = is_consistent && found;
+
+ inconsistency_explanation << "The constrained capture " << Co
+ << " 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);
+
}
// find constrained_by pointer in constrained feature
@@ -1673,7 +1799,13 @@ bool Problem::check(int verbose_level) const
if (verbose_level > 0)
cout << endl;
// check constrained_by pointer in constrained feature
- is_consistent = is_consistent && found;
+ // is_consistent = is_consistent && found;
+
+ 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);
}
// find constrained_by pointer in constrained landmark
@@ -1691,7 +1823,13 @@ bool Problem::check(int verbose_level) const
if (verbose_level > 0)
cout << endl;
// check constrained_by pointer in constrained landmark
- is_consistent = is_consistent && found;
+ // is_consistent = is_consistent && found;
+
+ inconsistency_explanation << "The constrained landmark " << Lo
+ << " 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);
}
if (verbose_level > 0)
{
@@ -1699,8 +1837,21 @@ bool Problem::check(int verbose_level) const
cout << " -> f" << c->getFeature()->id() << " @ " << c->getFeature().get() << endl;
}
// check problem and feature pointers
- is_consistent = is_consistent && (c->getProblem().get() == P_raw);
- is_consistent = is_consistent && (c->getFeature() == f);
+ // is_consistent = is_consistent && (c->getProblem().get() == P_raw);
+
+ inconsistency_explanation << "The factor " << 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()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
+ // is_consistent = is_consistent && (c->getFeature() == f);
+
+ inconsistency_explanation << "The factor " << c << " has feature ptr " << c->getFeature()
+ << " but it should have " << f << "\n";
+ log.compose(CheckLog((c->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ //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
@@ -1757,8 +1908,19 @@ bool Problem::check(int verbose_level) const
cout << " NOT FOUND !";
cout << endl;
}
+
+ 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);
+
// check that all state block pointers were found
- is_consistent = is_consistent && found;
+ // is_consistent = is_consistent && found;
}
}
}
@@ -1771,7 +1933,12 @@ bool Problem::check(int verbose_level) const
if (verbose_level > 0)
cout << "M @ " << M.get() << endl;
// check pointer to Problem
- is_consistent = is_consistent && (M->getProblem().get() == P_raw);
+ // is_consistent = is_consistent && (M->getProblem().get() == P_raw);
+
+ 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);
// Landmarks =======================================================================================
for (auto L : M->getLandmarkList())
@@ -1794,35 +1961,61 @@ bool Problem::check(int verbose_level) const
}
}
// check problem and map pointers
- is_consistent = is_consistent && (L->getProblem().get() == P_raw);
- is_consistent = is_consistent && (L->getMap() == M);
- for (auto cby : L->getConstrainedByList())
- {
- if (verbose_level > 0)
- cout << " <- c" << cby->id() << " -> L" << cby->getLandmarkOther()->id() << endl;
- // check constrained_by pointer to this landmark
- is_consistent = is_consistent && (cby->getLandmarkOther() && L->id() == cby->getLandmarkOther()->id());
- for (auto sb : cby->getStateBlockPtrVector())
- {
- if (verbose_level > 0)
- {
- cout << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- cout << endl;
- }
+ // is_consistent = is_consistent && (L->getProblem().get() == P_raw);
+
+ inconsistency_explanation << "The 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);
+
+ // is_consistent = is_consistent && (L->getMap() == M);
+
+ 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);
+
+ for (auto cby : L->getConstrainedByList()) {
+ if (verbose_level > 0)
+ cout << " <- c" << cby->id() << " -> L"
+ << cby->getLandmarkOther()->id() << endl;
+ // check constrained_by pointer to this landmark
+ // is_consistent =
+ // is_consistent && (cby->getLandmarkOther() &&
+ // L->id() == cby->getLandmarkOther()->id());
+
+ inconsistency_explanation << "The landmark constrained-by loop started at "
+ << cby
+ << " is not closed\n";
+ log.compose(CheckLog((cby->getLandmarkOther() &&
+ L->id() == cby->getLandmarkOther()->id()), inconsistency_explanation.str()));
+ // Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
+ for (auto sb : cby->getStateBlockPtrVector()) {
+ if (verbose_level > 0) {
+ cout << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ cout << " (lp @ " << lp.get() << ")";
+ }
+ cout << endl;
}
+ }
}
}
- if (verbose_level) cout << "--------------------------- Wolf tree " << (is_consistent ? " OK" : "Not OK !!") << endl;
+ if (verbose_level) cout << "--------------------------- Wolf tree " << (log.is_consistent_ ? " OK" : "Not OK !!") << endl;
if (verbose_level) cout << endl;
+ if (verbose_level and not log.is_consistent_) cout << log.explanation_ << endl;
- return is_consistent;
+ return log.is_consistent_;
}
void Problem::print(const std::string& depth, bool constr_by, bool metric, bool state_blocks) const
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 064659074cb5db4c8dfddd63b802e1616491491b..09afa700e79e66b058df609ea004334bd0830e4b 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -274,7 +274,7 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts) const
{
if ((*frame_rev_it)->getTimeStamp() <= _ts)
{
- CaptureBasePtr capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
if (capture)
// found the most recent Capture made by this sensor !
break;