diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 7eb1a10b19fe6ffcc169468c6feb69dd33aed69c..8811ed06d459893a6b738812361f008e0a30efa2 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -116,6 +116,9 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
std::ostream& stream = std::cout,
std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
protected:
virtual SizeEigen computeCalibSize() const;
diff --git a/include/core/common/node_base.h b/include/core/common/node_base.h
index 2a6120408718df85ebb2945c9800d71b0060b6fd..dcd7d37176c2eb4c36aea7c8b342aa064b43a896 100644
--- a/include/core/common/node_base.h
+++ b/include/core/common/node_base.h
@@ -3,6 +3,7 @@
// Wolf includes
#include "core/common/wolf.h"
+#include "core/utils/check_log.h"
namespace wolf {
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index aa0164605fc80ddd0b48915a3b75e6c69c6a1b8b..6436100b70dc72d24e5332c693792ae11f567330 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -220,6 +220,9 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
std::ostream& stream = std::cout,
std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
private:
void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 83e343c0c3f25aaeb0219289e143bfa8f7733fae..da1f68b8400cb94f87d1cc5ee460fbf8440b7bee 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -119,6 +119,9 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
std::ostream& stream = std::cout,
std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, FeatureBasePtr _feature_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
private:
void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
FactorBasePtr addFactor(FactorBasePtr _co_ptr);
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 32b582b04f15da321b6dd7710fda60ac9e882cac..e43dd50dc7e0161bbe16830e760145a914c277fa 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -144,6 +144,10 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
private:
CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h
index 3c29221acb417b3d0e13fb764207e2ae05e67190..fba575b0b8ea55a15b8bbe4972d0bc12984ae62b 100644
--- a/include/core/hardware/hardware_base.h
+++ b/include/core/hardware/hardware_base.h
@@ -40,6 +40,9 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
std::ostream& stream = std::cout,
std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, HardwareBasePtr _hwd_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
private:
virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
};
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 15b423c3b398230697f066fead211569ee1a36d2..3d177d79c972b1e072fb86d5d7790e686cd387ff 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -99,6 +99,9 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
std::ostream& stream = std::cout,
std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
private:
void setMap(const MapBasePtr _map_ptr);
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index 75a00a97c1a7026e51604ecca877f5b0e639f056..e609298014d8b68ca9e686a877ea80b041b34214 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -50,6 +50,9 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, MapBasePtr _map_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
std::string dateTimeNow();
};
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index b12d92f10a430810150040fe9277346d2f63c620..4182e762db050a3f36e714d453b73fb63568f5bc 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -377,6 +377,9 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, ProcessorBasePtr _prc_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
};
inline bool ProcessorBase::isVotingActive() const
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 2bf9d2bbc902622e984a88471b62782115088fec..e1be8095b90f395477ea72ed0302520fb0ae4bbb 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -255,6 +255,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
std::ostream& stream = std::cout,
std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
void link(HardwareBasePtr);
template<typename classType, typename... T>
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 8d6e69e41d741bd70bdc6e212df2490ecbc4b62c..f2153401cea96f3af77f360a55453200c16a7877 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -59,6 +59,9 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
void removeFrame(FrameBasePtr _frame_ptr);
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 65a78728260a755e3b9ddbc370d7d8bc623a3160..79c40b7604f4edbfb5c0cae8605493e63eada692 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -344,4 +344,105 @@ void CaptureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b
for (auto f : getFeatureList())
f->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
+
+CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
+ {
+ _stream << _tabs << "Cap" << id() << " @ " << _cap_ptr.get() << " -> Sen";
+ if (getSensor()) _stream << getSensor()->id();
+ else _stream << "-";
+ _stream << std::endl;
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Frm" << getFrame()->id() << " @ " << getFrame().get() << std::endl;
+ }
+ for (auto pair: getStateBlockMap())
+ {
+ auto sb = pair.second;
+
+ // check for valid state block
+ inconsistency_explanation << "Capture " << id() << " @ " << _cap_ptr.get() << " has State block pointer "
+ << sb.get() << " = 0 \n";
+ log.assertTrue((sb.get() != 0), inconsistency_explanation);
+
+ if (_verbose)
+ {
+ _stream << _tabs << " " << pair.first << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+
+ auto frm_ptr = getFrame();
+ // check problem and frame pointers
+ inconsistency_explanation << "Capture problem pointer " << getProblem().get()
+ << " different from frame problem pointer " << frm_ptr->getProblem().get() << "\n";
+ log.assertTrue((getProblem() == frm_ptr->getProblem()), inconsistency_explanation);
+
+
+ // check contrained_by
+ for (const auto& cby : getConstrainedByList())
+ {
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
+ for (const auto& Cow : cby->getCaptureOtherList())
+ _stream << " Cap" << Cow.lock()->id();
+ _stream << std::endl;
+ }
+
+ // check constrained_by pointer to this capture
+ inconsistency_explanation << "constrained by capture " << id() << " @ " << _cap_ptr.get()
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasCaptureOther(_cap_ptr)), inconsistency_explanation);
+
+ for (auto sb : cby->getStateBlockPtrVector())
+ {
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "sb @ " << sb.get();
+ if (sb)
+ {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+ }
+
+ auto frm_cap = _cap_ptr->getFrame();
+ inconsistency_explanation << "Cap" << id() << " @ " << _cap_ptr
+ << " ---> Frm" << frm_cap->id() << " @ " << frm_cap
+ << " -X-> Frm" << id();
+ auto frm_cap_list = frm_cap->getCaptureList();
+ auto frame_has_cap = std::find_if(frm_cap_list.begin(), frm_cap_list.end(), [&_cap_ptr](CaptureBasePtr cap){ return cap == _cap_ptr;});
+ log.assertTrue(frame_has_cap != frm_cap_list.end(), inconsistency_explanation);
+
+ for(auto f : getFeatureList())
+ {
+ inconsistency_explanation << "Cap " << id() << " @ " << _cap_ptr
+ << " ---> Ftr" << f->id() << " @ " << f
+ << " -X-> Cap" << id();
+ log.assertTrue((f->getCapture() == _cap_ptr), inconsistency_explanation);
+ }
+ return log;
+}
+bool CaptureBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto sen_ptr = std::static_pointer_cast<CaptureBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, sen_ptr, _stream, _tabs);
+ _log.compose(local_log);
+
+ for(auto f : getFeatureList()) f->check(_log, f, _verbose, _stream, _tabs + " ");
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index ee45aa68570db221c37b2d686c26aab948d6f40e..f85054ef85efb67e019d1720fb5c364b1774a4fc 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -336,4 +336,284 @@ void FactorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_bl
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
}
+
+CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
+ _stream << _tabs << "Fac" << id() << " @ " << _fac_ptr.get();
+
+ if ( getFrameOtherList() .empty()
+ && getCaptureOtherList() .empty()
+ && getFeatureOtherList() .empty()
+ && getLandmarkOtherList().empty() ) // case ABSOLUTE:
+ {
+ if (_verbose)
+ _stream << " --> Abs.";
+ }
+
+ // find constrained_by pointer in constrained frame
+ for (const auto& Fow : getFrameOtherList())
+ {
+ if (!Fow.expired())
+ {
+ const auto& Fo = Fow.lock();
+ if (_verbose)
+ {
+ _stream << " ( --> Frm" << Fo->id() << " <- ";
+ for (auto cby : Fo->getConstrainedByList())
+ _stream << " Fac" << cby->id();
+ }
+
+ // check constrained_by pointer in constrained frame
+ bool found = Fo->isConstrainedBy(_fac_ptr);
+ inconsistency_explanation << "The constrained Feature " << Fo->id() << " @ " << Fo
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+
+ }
+ }
+ if (_verbose && !getFrameOtherList().empty())
+ _stream << ")";
+ // find constrained_by pointer in constrained capture
+ for (const auto& Cow : getCaptureOtherList())
+ {
+ if (!Cow.expired())
+ {
+ const auto& Co = Cow.lock();
+
+ if (_verbose)
+ {
+ _stream << " ( --> Cap" << Co->id() << " <- ";
+ for (auto cby : Co->getConstrainedByList())
+ _stream << " Fac" << cby->id();
+ }
+
+ // check constrained_by pointer in constrained frame
+ bool found = Co->isConstrainedBy(_fac_ptr);
+ inconsistency_explanation << "The constrained capture " << Co->id() << " @ " << Co
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+
+ }
+ }
+ if (_verbose && !getCaptureOtherList().empty())
+ _stream << ")";
+ // find constrained_by pointer in constrained feature
+ for (const auto& fow : getFeatureOtherList())
+ {
+ if (!fow.expired())
+ {
+ const auto& fo = fow.lock();
+ if (_verbose)
+ {
+ _stream << " ( --> Ftr" << fo->id() << " <- ";
+ for (auto cby : fo->getConstrainedByList())
+ _stream << " Fac" << cby->id();
+ }
+
+ // check constrained_by pointer in constrained feature
+ bool found = fo->isConstrainedBy(_fac_ptr);
+ inconsistency_explanation << "The constrained feature " << fo->id() << " @ " << fo
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+ }
+ }
+ if (_verbose && !getFeatureOtherList().empty())
+ _stream << ")";
+
+ // find constrained_by pointer in constrained landmark
+ for (const auto& Low : getLandmarkOtherList())
+ {
+ if (Low.expired())
+ {
+ const auto& Lo = Low.lock();
+
+ if (_verbose)
+ {
+ _stream << " ( --> Lmk" << Lo->id() << " <- ";
+ for (auto cby : Lo->getConstrainedByList())
+ _stream << " Fac" << cby->id();
+ }
+
+ // check constrained_by pointer in constrained landmark
+ bool found = Lo->isConstrainedBy(_fac_ptr);
+ inconsistency_explanation << "The constrained landmark " << Lo->id() << " @ " << Lo
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+
+ }
+ }
+ if (_verbose && !getLandmarkOtherList().empty())
+ _stream << ")";
+ if (_verbose)
+ _stream << std::endl;
+ //Check Problem and feature ptrs
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Ftr" << getFeature()->id() << " @ " << getFeature().get() << std::endl;
+ }
+ auto ftr_ptr = getFeature();
+ // check problem and feature pointers
+ inconsistency_explanation << "The factor " << id() << " @ " << _fac_ptr.get() << " problem ptr " << getProblem().get()
+ << " is different from Feature's problem ptr " << ftr_ptr->getProblem().get() << "\n";
+ log.assertTrue((getProblem() == ftr_ptr->getProblem()), inconsistency_explanation);
+
+
+ inconsistency_explanation << "Fac" << id() << " @ " << _fac_ptr
+ << " ---> Ftr" << ftr_ptr->id() << " @ " << ftr_ptr
+ << " -X-> Fac" << id();
+ auto ftr_fac_list = ftr_ptr->getFactorList();
+ auto ftr_has_fac = std::find_if(ftr_fac_list.begin(), ftr_fac_list.end(), [&_fac_ptr](FactorBasePtr fac){ return fac == _fac_ptr;});
+ log.assertTrue(ftr_has_fac!= ftr_fac_list.end(), inconsistency_explanation);
+
+ // find state block pointers in all constrained nodes
+ SensorBasePtr S = getSensor(); // get own sensor to check sb
+ FrameBasePtr F = getFrame();
+ CaptureBasePtr C = getCapture();
+
+ for (auto sb : getStateBlockPtrVector())
+ {
+ bool found = false;
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "sb @ " << sb.get();
+ if (sb)
+ {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ }
+ bool found_here;
+
+ // find in own Frame
+ found_here = F->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Frm" << F->id();
+ found = found || found_here;
+
+ // find in own Capture
+ found_here = C->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Cap" << C->id();
+ found = found || found_here;
+
+ // Find in other Captures of the own Frame
+ if (!found_here)
+ for (auto FC : F->getCaptureList())
+ {
+ found_here = FC->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Frm" << F->id() << ".Cap" << FC->id();
+ found = found || found_here;
+ }
+
+ // find in own Sensor
+ if (S)
+ {
+ found_here = S->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Sen" << S->id();
+ found = found || found_here;
+ }
+
+
+ // find in constrained Frame
+ for (const auto& Fow : getFrameOtherList())
+ {
+ if (!Fow.expired())
+ {
+ const auto& Fo = Fow.lock();
+ found_here = Fo->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " FrmO" << Fo->id();
+ found = found || found_here;
+
+ // find in feature other's captures
+ for (auto FoC : Fo->getCaptureList())
+ {
+ found_here = FoC->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " FrmO" << Fo->id() << ".C" << FoC->id();
+ found = found || found_here;
+ }
+
+ }
+ }
+
+ // find in constrained Capture
+ for (const auto& Cow : getCaptureOtherList())
+ {
+ if (!Cow.expired())
+ {
+ const auto& Co = Cow.lock();
+ found_here = Co->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " CapO" << Co->id();
+ found = found || found_here;
+ }
+ }
+
+ // find in constrained Feature
+ for (const auto& fow : getFeatureOtherList())
+ {
+ if (!fow.expired())
+ {
+ const auto& fo = fow.lock();
+ // find in constrained feature's Frame
+ auto foF = fo->getFrame();
+ found_here = foF->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " FtrOF" << foF->id();
+ found = found || found_here;
+
+ // find in constrained feature's Capture
+ auto foC = fo->getCapture();
+ found_here = foC->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " FtrOC" << foC->id();
+ found = found || found_here;
+
+ // find in constrained feature's Sensor
+ auto foS = fo->getCapture()->getSensor();
+ found_here = foS->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " FtrOS" << foS->id();
+ found = found || found_here;
+ }
+ }
+
+ // find in constrained landmark
+ for (const auto& Low : getLandmarkOtherList())
+ {
+ if (!Low.expired())
+ {
+ const auto& Lo = Low.lock();
+ found_here = Lo->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " LmkO" << Lo->id();
+ found = found || found_here;
+ }
+ }
+
+ if (_verbose)
+ {
+ if (found)
+ _stream << " found";
+ else
+ _stream << " NOT FOUND !";
+ _stream << std::endl;
+ }
+
+ // check that the state block has been found somewhere
+ inconsistency_explanation << "The stateblock " << sb << " has not been found (is floating!)";
+ log.assertTrue((found), inconsistency_explanation);
+
+ inconsistency_explanation << "The stateblock " << sb << " of factor " << id() << " @ " << _fac_ptr << " is null\n";
+ log.assertTrue((sb.get() != nullptr), inconsistency_explanation);
+ }
+ return log;
+}
+
+bool FactorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto fac_ptr = std::static_pointer_cast<FactorBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, fac_ptr, _stream, _tabs);
+ _log.compose(local_log);
+
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index d3aa372b7ecdae23e6d09382f12e7fd22ff7df58..0acccaf7a9fb9404e76ac9c5b142ed0839a9405c 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -188,4 +188,70 @@ void FeatureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b
for (auto c : getFactorList())
c->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
+
+CheckLog FeatureBase::localCheck(bool _verbose, FeatureBasePtr _ftr_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
+ {
+ _stream << _tabs << "Ftr" << id() << " @ " << _ftr_ptr.get() << std::endl;
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Cap" << getCapture()->id() << " @ " << getCapture().get()
+ << std::endl;
+ }
+
+ auto cap_ptr = getCapture();
+ // check problem and capture pointers
+ inconsistency_explanation << "Feature frame problem pointer " << getProblem().get()
+ << " different from Capture problem pointer " << cap_ptr->getProblem().get() << "\n";
+ log.assertTrue((getProblem() == cap_ptr->getProblem()), inconsistency_explanation);
+
+
+ // check contrained_by
+ for (auto cby : getConstrainedByList())
+ {
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
+ for (const auto& fow : cby->getFeatureOtherList())
+ _stream << " Ftr" << fow.lock()->id();
+ _stream << std::endl;
+ }
+
+ // check constrained_by pointer to this feature
+ inconsistency_explanation << "constrained by Feature " << id() << " @ " << _ftr_ptr.get()
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasFeatureOther(_ftr_ptr)), inconsistency_explanation);
+
+ }
+
+ auto cap_ftr = _ftr_ptr->getCapture();
+ inconsistency_explanation << "Ftr" << id() << " @ " << _ftr_ptr
+ << " ---> Cap" << cap_ftr->id() << " @ " << cap_ftr
+ << " -X-> Ftr" << id();
+ auto cap_ftr_list = cap_ftr->getFeatureList();
+ auto frame_has_cap = std::find_if(cap_ftr_list.begin(), cap_ftr_list.end(), [&_ftr_ptr](FeatureBasePtr ftr){ return ftr == _ftr_ptr;});
+ log.assertTrue(frame_has_cap != cap_ftr_list.end(), inconsistency_explanation);
+
+ for(auto fac : getFactorList())
+ {
+ inconsistency_explanation << "Ftr" << id() << " @ " << _ftr_ptr
+ << " ---> Fac" << fac->id() << " @ " << fac
+ << " -X-> Ftr" << id();
+ log.assertTrue((fac->getFeature() == _ftr_ptr), inconsistency_explanation);
+ }
+ return log;
+}
+bool FeatureBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto ftr_ptr = std::static_pointer_cast<FeatureBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, ftr_ptr, _stream, _tabs);
+ _log.compose(local_log);
+
+ for(auto f : getFactorList()) f->check(_log, f, _verbose, _stream, _tabs + " ");
+
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index a36417929bab661538736d837b016683c9597792..bca65f5a4bb2e7b11e0e48fd54013f265dd8db95 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -403,4 +403,107 @@ void FrameBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blo
for (auto C : getCaptureList())
C->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
+
+CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose) {
+ _stream << _tabs << (isKeyOrAux() ? (isKey() ? "KFrm" : "EFrm") : "Frm")
+ << id() << " @ " << _frm_ptr.get() << std::endl;
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Trj @ " << getTrajectory().get() << std::endl;
+ }
+ for (const auto &pair: getStateBlockMap()) {
+
+ auto sb = pair.second;
+
+ // check for valid state block
+ inconsistency_explanation << "Frame " << id() << " @ "<< _frm_ptr.get()
+ << " has State block pointer " << sb.get()
+ << " = 0 \n";
+ log.assertTrue((sb.get() != 0), inconsistency_explanation);
+
+ if (_verbose) {
+ _stream << _tabs << " " << pair.first << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+
+ // check problem pointer
+ auto trajectory_ptr = getTrajectory();
+ auto trajectory_problem_ptr = trajectory_ptr->getProblem();
+ inconsistency_explanation << "Frame problem pointer " << getProblem().get()
+ << " different from Trajectory problem pointer " << trajectory_problem_ptr.get() << "\n";
+ log.assertTrue((getProblem() == trajectory_problem_ptr), inconsistency_explanation);
+
+ // // check trajectory pointer
+ // inconsistency_explanation << "Frame trajectory pointer is " << getTrajectory()
+ // << " but trajectory pointer is" << trajectory_ptr << "\n";
+ // log.assertTrue((getTrajectory() == T, inconsistency_explanation);
+
+ // check constrained_by
+ for (auto cby : getConstrainedByList())
+ {
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
+ for (const auto& Fow : cby->getFrameOtherList())
+ _stream << " Frm" << Fow.lock()->id() << std::endl;
+
+
+ // check constrained_by pointer to this frame
+ inconsistency_explanation << "constrained-by frame " << id() << " @ " << _frm_ptr
+ << " not found among constrained-by factors\n";
+ auto F = std::static_pointer_cast<FrameBase>(_frm_ptr);
+ log.assertTrue((cby->hasFrameOther(F)), inconsistency_explanation);
+
+ for (auto sb : cby->getStateBlockPtrVector())
+ {
+ if (_verbose) {
+ _stream << _tabs << " " << "sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+ }
+ }
+
+ auto trj_ptr = getTrajectory();
+ inconsistency_explanation << "Frm" << id() << " @ " << _frm_ptr
+ << " ---> Trj" << " @ " << trj_ptr
+ << " -X-> Frm" << id();
+ auto trj_frm_list = trj_ptr->getFrameList();
+ auto trj_has_frm = std::find_if(trj_frm_list.begin(), trj_frm_list.end(), [&_frm_ptr](FrameBasePtr frm){ return frm == _frm_ptr;});
+ log.assertTrue(trj_has_frm != trj_frm_list.end(), inconsistency_explanation);
+
+ for(auto C : getCaptureList())
+ {
+ inconsistency_explanation << "Frm " << id() << " @ " << _frm_ptr
+ << " ---> Cap" << C->id() << " @ " << C
+ << " -X-> Frm" << id();
+ log.assertTrue((C->getFrame() == _frm_ptr), inconsistency_explanation);
+ }
+ return log;
+}
+
+bool FrameBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto frm_ptr = std::static_pointer_cast<FrameBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, frm_ptr, _stream, _tabs);
+ _log.compose(local_log);
+ for(auto C : getCaptureList()) C->check(_log, C, _verbose, _stream, _tabs + " ");
+
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index 788f644c00298bfb7d7108271a2159475bc70334..72f07b8e216e6ca830ec9021dbb7ca1144d09860 100644
--- a/src/hardware/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -32,4 +32,29 @@ void HardwareBase::print(int _depth, bool _constr_by, bool _metric, bool _state_
for (auto S : getSensorList())
S->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
+
+CheckLog HardwareBase::localCheck(bool _verbose, HardwareBasePtr _hwd_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+ if (_verbose)
+ {
+ _stream << _tabs << "Hrw @ " << _hwd_ptr.get() << std::endl;
+ }
+
+ // check pointer to Problem
+ inconsistency_explanation << "Hwd->getProblem() [" << getProblem().get()
+ << "] -> " << " Prb->getHardware() [" << getProblem()->getHardware().get() << "] -> Hwd [" << _hwd_ptr.get() << "] Mismatch!\n";
+ log.assertTrue((_hwd_ptr->getProblem()->getHardware().get() == _hwd_ptr.get()), inconsistency_explanation);
+ return log;
+}
+bool HardwareBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto hwd_ptr = std::static_pointer_cast<HardwareBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, hwd_ptr, _stream, _tabs);
+ _log.compose(local_log);
+ for (auto S : getSensorList())
+ S->check(_log, S, _verbose, _stream, _tabs + " ");
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 56cf45857c880907415709525784ba50b85118fe..9f2552615dc76f2c631b3007578da62bcfbea462 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -219,6 +219,90 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
return lmk;
}
+CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
+ {
+ _stream << _tabs << "Lmk" << id() << " @ " << _lmk_ptr.get() << std::endl;
+ _stream << _tabs << " -> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " -> Map @ " << getMap().get() << std::endl;
+ for (const auto& pair : getStateBlockMap())
+ {
+ auto sb = pair.second;
+ _stream << _tabs << " " << pair.first << " sb @ " << sb.get();
+ if (sb)
+ {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+
+ auto map_ptr = getMap();
+ // check problem and map pointers
+ inconsistency_explanation << "Landmarks' problem ptr "
+ << getProblem().get() << " different from Map's problem ptr "
+ << map_ptr->getProblem().get() << "\n";
+
+ log.assertTrue((getProblem() == map_ptr->getProblem()), inconsistency_explanation);
+
+ for (auto cby : getConstrainedByList())
+ {
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "<- Fac" << cby->id() << " ->";
+
+ for (const auto& Low : cby->getLandmarkOtherList())
+ {
+ if (!Low.expired())
+ {
+ const auto& Lo = Low.lock();
+ _stream << " Lmk" << Lo->id();
+ }
+ }
+ _stream << std::endl;
+ }
+
+ // check constrained-by factors
+ inconsistency_explanation << "constrained-by landmark " << id() << " @ " << _lmk_ptr.get()
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasLandmarkOther(_lmk_ptr)), inconsistency_explanation);
+
+ for (auto sb : cby->getStateBlockPtrVector()) {
+ if (_verbose) {
+ _stream << _tabs << " " << "sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+ }
+
+ inconsistency_explanation << "Lmk" << id() << " @ " << _lmk_ptr
+ << " ---> Map" << map_ptr
+ << " -X-> Lmk" << id();
+ auto map_lmk_list = map_ptr->getLandmarkList();
+ auto map_has_lmk = std::find_if(map_lmk_list.begin(), map_lmk_list.end(), [&_lmk_ptr](LandmarkBasePtr lmk){ return lmk == _lmk_ptr;});
+ log.assertTrue(map_has_lmk != map_lmk_list.end(), inconsistency_explanation);
+
+ return log;
+}
+bool LandmarkBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto lmk_ptr = std::static_pointer_cast<LandmarkBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, lmk_ptr, _stream, _tabs);
+ _log.compose(local_log);
+
+ return _log.is_consistent_;
+}
// Register landmark creator
namespace
{
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 73efc29d7497f9e7c3fa434cc62cfef611ff9277..4c13360effa052b61b189042a95d2ec92bedfa4d 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -101,4 +101,28 @@ void MapBase::print(int _depth, bool _constr_by, bool _metric, bool _state_block
for (auto L : getLandmarkList())
L->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
+
+CheckLog MapBase::localCheck(bool _verbose, MapBasePtr _map_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
+ _stream << _tabs << "Map @ " << _map_ptr.get() << std::endl;
+
+ // check pointer to Problem
+ inconsistency_explanation << "Map->getProblem() [" << getProblem().get()
+ << "] -> " << " Prb->getMap() [" << getProblem()->getMap().get() << "] -> Map [" << _map_ptr.get() << "] Mismatch!\n";
+ log.assertTrue((_map_ptr->getProblem()->getMap().get() == _map_ptr.get()), inconsistency_explanation);
+ return log;
+}
+bool MapBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto map_ptr = std::static_pointer_cast<MapBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, map_ptr, _stream, _tabs);
+ _log.compose(local_log);
+ for (auto L : getLandmarkList())
+ L->check(_log, L, _verbose, _stream, _tabs + " ");
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index f2d41a420415f8b0f9fb3ac9f6b5e286036c1160..58112a40d249aaadaad330dbf9ae08fe05263e85 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -1067,6 +1067,8 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
CheckLog log(true, "");
log.explanation_ = "## WOLF::problem inconsistencies list \n ---------------------------------- \n";
+ std::string tabs = "";
+
if (_verbose) _stream << std::endl;
if (_verbose) _stream << "Wolf tree integrity ---------------------" << std::endl;
auto P = shared_from_this();
@@ -1078,624 +1080,19 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// HARDWARE branch
// ------------------------
auto H = hardware_ptr_;
- if (_verbose)
- {
- _stream << "Hrw @ " << H.get() << std::endl;
- }
-
- // check pointer to Problem
- std::stringstream inconsistency_explanation;
- inconsistency_explanation << "Hardware problem pointer is " << H->getProblem().get()
- << " but problem pointer is " << P.get() << "\n";
- log.assertTrue((H->getProblem() == P), inconsistency_explanation);
-
-
- // Sensors =======================================================================================
- for (auto S : H->getSensorList())
- {
- if (_verbose)
- {
- _stream << " Sen" << S->id() << " @ " << S.get() << std::endl;
- _stream << " -> Prb @ " << S->getProblem().get() << std::endl;
- _stream << " -> Hrw @ " << S->getHardware().get() << std::endl;
- for (auto pair: S->getStateBlockMap())
- {
- auto sb = pair.second;
- _stream << " " << pair.first << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
-
- // check problem and hardware pointers
- inconsistency_explanation << "Sensor problem pointer is " << S->getProblem().get()
- << " but problem pointer is " << P.get() << "\n";
- log.assertTrue((S->getProblem() == P), inconsistency_explanation);
-
-
- inconsistency_explanation << "Sensor hardware pointer is " << S->getHardware()
- << " but hardware pointer is " << H << "\n";
- log.assertTrue((S->getHardware() == H), inconsistency_explanation);
-
-
- // Processors =======================================================================================
- for (auto p : S->getProcessorList())
- {
- if (_verbose)
- {
- _stream << " Prc" << p->id() << " @ " << p.get() << " -> Sen" << p->getSensor()->id() << std::endl;
- _stream << " -> Prb @ " << p->getProblem().get() << std::endl;
- _stream << " -> Sen" << p->getSensor()->id() << " @ " << p->getSensor().get() << std::endl;
- }
-
- // check problem and sensor pointers
- inconsistency_explanation << "Processor problem pointer is " << p->getProblem().get()
- << " but problem pointer is " << P.get() << "\n";
- log.assertTrue((p->getProblem() == P), inconsistency_explanation);
-
-
- inconsistency_explanation << "Processor sensor pointer is " << p->getSensor()
- << " but sensor pointer is " << P.get() << "\n";
- log.assertTrue((p->getProblem() == P), inconsistency_explanation);
-
- }
- }
+ H->check(log, H, _verbose, _stream, tabs);
// ------------------------
// TRAJECTORY branch
// ------------------------
auto T = trajectory_ptr_;
- if (_verbose)
- {
- _stream << "Trj @ " << T.get() << std::endl;
- }
-
- // check pointer to Problem
- inconsistency_explanation << "Trajectory problem pointer is " << T->getProblem().get()
- << " but problem pointer is" << P.get() << "\n";
- log.assertTrue((T->getProblem() == P), inconsistency_explanation);
-
-
- // Frames =======================================================================================
- for (auto F : T->getFrameList())
- {
- if (_verbose) {
- _stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " EFrm") : " Frm")
- << F->id() << " @ " << F.get() << std::endl;
- _stream << " -> Prb @ " << F->getProblem().get() << std::endl;
- _stream << " -> Trj @ " << F->getTrajectory().get() << std::endl;
- }
- for (const auto &pair: F->getStateBlockMap()) {
-
- auto sb = pair.second;
-
- // check for valid state block
- inconsistency_explanation << "Frame " << F->id() << " @ "<< F.get()
- << " has State block pointer " << sb.get()
- << " = 0 \n";
- log.assertTrue((sb.get() != 0), inconsistency_explanation);
-
- if (_verbose) {
- _stream << " "<< pair.first << " sb @ " << sb.get();
- if (sb) {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
-
- // check problem pointer
- inconsistency_explanation << "Frame problem pointer is " << F->getProblem().get()
- << " but problem pointer is" << P.get() << "\n";
- log.assertTrue((F->getProblem() == P), inconsistency_explanation);
-
- // check trajectory pointer
- inconsistency_explanation << "Frame trajectory pointer is " << F->getTrajectory()
- << " but trajectory pointer is" << T << "\n";
- log.assertTrue((F->getTrajectory() == T), inconsistency_explanation);
-
-
- // check constrained_by
- for (auto cby : F->getConstrainedByList())
- {
- if (_verbose)
- {
- _stream << " <- Fac" << cby->id() << " -> ";
- for (const auto& Fow : cby->getFrameOtherList())
- _stream << " F" << Fow.lock()->id() << std::endl;
- }
-
- // check constrained_by pointer to this frame
- inconsistency_explanation << "constrained-by frame " << F->id() << " @ " << F
- << " not found among constrained-by factors\n";
- log.assertTrue((cby->hasFrameOther(F)), inconsistency_explanation);
-
- for (auto sb : cby->getStateBlockPtrVector())
- {
-
- if (_verbose) {
- _stream << " sb @ " << sb.get();
- if (sb) {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
- }
-
- // Captures =======================================================================================
- for (auto C : F->getCaptureList())
- {
- if (_verbose)
- {
- _stream << " Cap" << C->id() << " @ " << C.get() << " -> Sen";
- if (C->getSensor()) _stream << C->getSensor()->id();
- else _stream << "-";
- _stream << std::endl;
- _stream << " -> Prb @ " << C->getProblem().get() << std::endl;
- _stream << " -> Frm" << C->getFrame()->id() << " @ " << C->getFrame().get() << std::endl;
- }
- for (auto pair: C->getStateBlockMap())
- {
- auto sb = pair.second;
-
- // check for valid state block
- inconsistency_explanation << "Capture " << C->id() << " @ " << C.get() << " has State block pointer "
- << sb.get() << " = 0 \n";
- log.assertTrue((sb.get() != 0), inconsistency_explanation);
-
- if (_verbose)
- {
- _stream << " " << pair.first << " sb @ " << sb.get();
- if (sb) {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
-
- // check problem and frame pointers
- inconsistency_explanation << "Capture problem pointer is " << C->getProblem().get()
- << " but problem pointer is" << P.get() << "\n";
- log.assertTrue((C->getProblem() == P), inconsistency_explanation);
+ T->check(log, T, _verbose, _stream, tabs);
-
- inconsistency_explanation << "Capture frame pointer is " << C->getFrame()
- << " but frame pointer is" << F << "\n";
- log.assertTrue((C->getFrame() == F), inconsistency_explanation);
-
-
- // check contrained_by
- for (const auto& cby : C->getConstrainedByList())
- {
- if (_verbose)
- {
- _stream << " <- Fac" << cby->id() << " -> ";
- for (const auto& Cow : cby->getCaptureOtherList())
- _stream << " Cap" << Cow.lock()->id();
- _stream << std::endl;
- }
-
- // check constrained_by pointer to this capture
- inconsistency_explanation << "constrained by capture " << C->id() << " @ " << C
- << " not found among constrained-by factors\n";
- log.assertTrue((cby->hasCaptureOther(C)), inconsistency_explanation);
-
- for (auto sb : cby->getStateBlockPtrVector())
- {
- if (_verbose)
- {
- _stream << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
-
- }
-
- // Features =======================================================================================
- for (auto f : C->getFeatureList())
- {
- if (_verbose)
- {
- _stream << " Ftr" << f->id() << " @ " << f.get() << std::endl;
- _stream << " -> Prb @ " << f->getProblem().get() << std::endl;
- _stream << " -> Cap" << f->getCapture()->id() << " @ " << f->getCapture().get()
- << std::endl;
- }
-
- // check problem and capture pointers
- inconsistency_explanation << "Feature frame pointer is " << f->getProblem().get()
- << " but problem pointer is" << P.get() << "\n";
- log.assertTrue((f->getProblem() == P), inconsistency_explanation);
-
-
- inconsistency_explanation << "Feature capture pointer is " << f->getCapture()
- << " but capture pointer is" << C << "\n";
- log.assertTrue((f->getCapture() == C), inconsistency_explanation);
-
-
- // check contrained_by
- for (auto cby : f->getConstrainedByList())
- {
- if (_verbose)
- {
- _stream << " <- Fac" << cby->id() << " -> ";
- for (const auto& fow : cby->getFeatureOtherList())
- _stream << " Ftr" << fow.lock()->id();
- _stream << std::endl;
- }
-
- // check constrained_by pointer to this feature
- inconsistency_explanation << "constrained by Feature " << f->id() << " @ " << f
- << " not found among constrained-by factors\n";
- log.assertTrue((cby->hasFeatureOther(f)), inconsistency_explanation);
-
- }
-
- // Factors =======================================================================================
- for (auto c : f->getFactorList())
- {
- if (_verbose)
- _stream << " Fac" << c->id() << " @ " << c.get();
-
- if ( c->getFrameOtherList() .empty()
- && c->getCaptureOtherList() .empty()
- && c->getFeatureOtherList() .empty()
- && c->getLandmarkOtherList().empty() ) // case ABSOLUTE:
- {
- if (_verbose)
- _stream << " --> Abs.";
- }
-
- // find constrained_by pointer in constrained frame
- for (const auto& Fow : c->getFrameOtherList())
- {
- if (!Fow.expired())
- {
- const auto& Fo = Fow.lock();
- if (_verbose)
- {
- _stream << " ( --> Frm" << Fo->id() << " <- ";
- for (auto cby : Fo->getConstrainedByList())
- _stream << " Fac" << cby->id();
- }
-
- // check constrained_by pointer in constrained frame
- bool found = Fo->isConstrainedBy(c);
- inconsistency_explanation << "The constrained Feature " << Fo->id() << " @ " << Fo
- << " not found among constrained-by factors\n";
- log.assertTrue((found), inconsistency_explanation);
-
- }
- }
- if (_verbose && !c->getFrameOtherList().empty())
- _stream << ")";
-
- // find constrained_by pointer in constrained capture
- for (const auto& Cow : c->getCaptureOtherList())
- {
- if (!Cow.expired())
- {
- const auto& Co = Cow.lock();
-
- if (_verbose)
- {
- _stream << " ( --> Cap" << Co->id() << " <- ";
- for (auto cby : Co->getConstrainedByList())
- _stream << " Fac" << cby->id();
- }
-
- // check constrained_by pointer in constrained frame
- bool found = Co->isConstrainedBy(c);
- inconsistency_explanation << "The constrained capture " << Co->id() << " @ " << Co
- << " not found among constrained-by factors\n";
- log.assertTrue((found), inconsistency_explanation);
-
- }
- }
- if (_verbose && !c->getCaptureOtherList().empty())
- _stream << ")";
-
- // find constrained_by pointer in constrained feature
- for (const auto& fow : c->getFeatureOtherList())
- {
- if (!fow.expired())
- {
- const auto& fo = fow.lock();
- if (_verbose)
- {
- _stream << " ( --> Ftr" << fo->id() << " <- ";
- for (auto cby : fo->getConstrainedByList())
- _stream << " Fac" << cby->id();
- }
-
- // check constrained_by pointer in constrained feature
- bool found = fo->isConstrainedBy(c);
- inconsistency_explanation << "The constrained feature " << fo->id() << " @ " << fo
- << " not found among constrained-by factors\n";
- log.assertTrue((found), inconsistency_explanation);
- }
- }
- if (_verbose && !c->getFeatureOtherList().empty())
- _stream << ")";
-
- // find constrained_by pointer in constrained landmark
- for (const auto& Low : c->getLandmarkOtherList())
- {
- if (Low.expired())
- {
- const auto& Lo = Low.lock();
-
- if (_verbose)
- {
- _stream << " ( --> Lmk" << Lo->id() << " <- ";
- for (auto cby : Lo->getConstrainedByList())
- _stream << " Fac" << cby->id();
- }
-
- // check constrained_by pointer in constrained landmark
- bool found = Lo->isConstrainedBy(c);
- inconsistency_explanation << "The constrained landmark " << Lo->id() << " @ " << Lo
- << " not found among constrained-by factors\n";
- log.assertTrue((found), inconsistency_explanation);
-
- }
- }
- if (_verbose && !c->getLandmarkOtherList().empty())
- _stream << ")";
-
- if (_verbose)
- _stream << std::endl;
-
- if (_verbose)
- {
- _stream << " -> Prb @ " << c->getProblem().get() << std::endl;
- _stream << " -> Ftr" << c->getFeature()->id() << " @ " << c->getFeature().get() << std::endl;
- }
-
- // check problem and feature pointers
- inconsistency_explanation << "The factor " << c->id() << " @ " << c << " has problem ptr " << c->getProblem().get()
- << " but problem ptr is " << P.get() << "\n";
- log.assertTrue((c->getProblem() == P), inconsistency_explanation);
-
-
- inconsistency_explanation << "The factor " << c->id() << " @ " << c << " has feature ptr " << c->getFeature()
- << " but it should have " << f << "\n";
- log.assertTrue((c->getProblem() == P), inconsistency_explanation);
-
-
- // find state block pointers in all constrained nodes
- SensorBasePtr S = c->getFeature()->getCapture()->getSensor(); // get own sensor to check sb
- for (auto sb : c->getStateBlockPtrVector())
- {
- bool found = false;
- if (_verbose)
- {
- _stream << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- }
- bool found_here;
-
- // find in own Frame
- found_here = F->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " Frm" << F->id();
- found = found || found_here;
-
- // find in own Capture
- found_here = C->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " Cap" << C->id();
- found = found || found_here;
-
- // Find in other Captures of the own Frame
- if (!found_here)
- for (auto FC : F->getCaptureList())
- {
- found_here = FC->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " Frm" << F->id() << ".Cap" << FC->id();
- found = found || found_here;
- }
-
- // find in own Sensor
- if (S)
- {
- found_here = S->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " Sen" << S->id();
- found = found || found_here;
- }
-
-
- // find in constrained Frame
- for (const auto& Fow : c->getFrameOtherList())
- {
- if (!Fow.expired())
- {
- const auto& Fo = Fow.lock();
- found_here = Fo->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " FrmO" << Fo->id();
- found = found || found_here;
-
- // find in feature other's captures
- for (auto FoC : Fo->getCaptureList())
- {
- found_here = FoC->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " FrmO" << Fo->id() << ".C" << FoC->id();
- found = found || found_here;
- }
-
- }
- }
-
- // find in constrained Capture
- for (const auto& Cow : c->getCaptureOtherList())
- {
- if (!Cow.expired())
- {
- const auto& Co = Cow.lock();
- found_here = Co->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " CapO" << Co->id();
- found = found || found_here;
- }
- }
-
- // find in constrained Feature
- for (const auto& fow : c->getFeatureOtherList())
- {
- if (!fow.expired())
- {
- const auto& fo = fow.lock();
- // find in constrained feature's Frame
- auto foF = fo->getFrame();
- found_here = foF->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " FtrOF" << foF->id();
- found = found || found_here;
-
- // find in constrained feature's Capture
- auto foC = fo->getCapture();
- found_here = foC->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " FtrOC" << foC->id();
- found = found || found_here;
-
- // find in constrained feature's Sensor
- auto foS = fo->getCapture()->getSensor();
- found_here = foS->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " FtrOS" << foS->id();
- found = found || found_here;
- }
- }
-
- // find in constrained landmark
- for (const auto& Low : c->getLandmarkOtherList())
- {
- if (!Low.expired())
- {
- const auto& Lo = Low.lock();
- found_here = Lo->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " LmkO" << Lo->id();
- found = found || found_here;
- }
- }
-
- if (_verbose)
- {
- if (found)
- _stream << " found";
- else
- _stream << " NOT FOUND !";
- _stream << std::endl;
- }
-
- // check that the state block has been found somewhere
- inconsistency_explanation << "The stateblock " << sb << " has not been found (is floating!)";
- log.assertTrue((found), inconsistency_explanation);
-
- inconsistency_explanation << "The stateblock " << sb << " of factor " << c->id() << " @ " << c << " is null\n";
- log.assertTrue((sb.get() != nullptr), inconsistency_explanation);
- }
- }
- }
- }
- }
// ------------------------
// MAP branch
// ------------------------
auto M = map_ptr_;
- if (_verbose)
- _stream << "Map @ " << M.get() << std::endl;
-
- // check pointer to Problem
- inconsistency_explanation << "The map problem ptr is " << M->getProblem().get() << " but problem is " << P.get() << "\n";
- log.assertTrue((M->getProblem() == P), inconsistency_explanation);
+ M->check(log, M, _verbose, _stream, tabs);
- // Landmarks =======================================================================================
- for (auto L : M->getLandmarkList())
- {
- if (_verbose)
- {
- _stream << " Lmk" << L->id() << " @ " << L.get() << std::endl;
- _stream << " -> Prb @ " << L->getProblem().get() << std::endl;
- _stream << " -> Map @ " << L->getMap().get() << std::endl;
- for (const auto& pair : L->getStateBlockMap())
- {
- auto sb = pair.second;
- _stream << " " << pair.first << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
-
- // check problem and map pointers
- inconsistency_explanation << "Landmarks' problem ptr is "
- << L->getProblem().get() << " but problem is "
- << P.get() << "\n";
-
- log.assertTrue((L->getProblem() == P), inconsistency_explanation);
-
- inconsistency_explanation << "The Landmarks' map ptr is "
- << L->getMap() << " but map is "
- << M << "\n";
- log.assertTrue((M->getProblem() == P), inconsistency_explanation);
-
- for (auto cby : L->getConstrainedByList())
- {
- if (_verbose)
- {
- _stream << " <- Fac" << cby->id() << " ->";
-
- for (const auto& Low : cby->getLandmarkOtherList())
- {
- if (!Low.expired())
- {
- const auto& Lo = Low.lock();
- _stream << " Lmk" << Lo->id();
- }
- }
- _stream << std::endl;
- }
-
- // check constrained-by factors
- inconsistency_explanation << "constrained-by landmark " << L->id() << " @ " << L
- << " not found among constrained-by factors\n";
- log.assertTrue((cby->hasLandmarkOther(L)), inconsistency_explanation);
-
- for (auto sb : cby->getStateBlockPtrVector()) {
- if (_verbose) {
- _stream << " sb @ " << sb.get();
- if (sb) {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
- }
- }
if (_verbose) _stream << "--------------------------- Wolf tree " << (log.is_consistent_ ? " OK" : "Not OK !!") << std::endl;
if (_verbose) _stream << std::endl;
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 6f28aef4d3393d1baf4a652fa4a06052ba9abb82..c02228864e1db24fb0b43826736a26c1c6688451 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -227,4 +227,38 @@ void ProcessorBase::print(int _depth, bool _metric, bool _state_blocks, bool _co
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
}
+CheckLog ProcessorBase::localCheck(bool _verbose, ProcessorBasePtr _prc_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ auto sen_ptr = getSensor();
+ if (_verbose)
+ {
+ _stream << _tabs << "Prc" << id() << " @ " << _prc_ptr.get() << " -> Sen" << sen_ptr->id() << std::endl;
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Sen" << sen_ptr->id() << " @ " << sen_ptr.get() << std::endl;
+ }
+
+ // check problem and sensor pointers
+ inconsistency_explanation << "Processor problem pointer " << getProblem().get()
+ << " is different from Sensor problem pointer " << sen_ptr->getProblem().get() << "\n";
+ log.assertTrue((getProblem() == sen_ptr->getProblem()), inconsistency_explanation);
+
+ inconsistency_explanation << "Prc" << id() << " @ " << _prc_ptr
+ << " ---> Sen" << sen_ptr->id() << " @ " << sen_ptr
+ << " -X-> Prc" << id();
+ auto sen_prc_list = sen_ptr->getProcessorList();
+ auto sen_has_prc = std::find_if(sen_prc_list.begin(), sen_prc_list.end(), [&_prc_ptr](ProcessorBasePtr prc){ return prc == _prc_ptr;});
+ log.assertTrue(sen_has_prc != sen_prc_list.end(), inconsistency_explanation);
+
+ return log;
+}
+bool ProcessorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto prc_ptr = std::static_pointer_cast<ProcessorBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, prc_ptr, _stream, _tabs);
+ _log.compose(local_log);
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index f40e6a4e5ca10d9d5369f06ce6e683e97c767ccc..2ae05abfde3dd647d6fca8ce9508f56607b29785 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -517,4 +517,61 @@ void SensorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_bl
for (auto p : getProcessorList())
p->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
+
+CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ if (_verbose)
+ {
+ _stream << _tabs << "Sen" << id() << " @ " << _sen_ptr.get() << std::endl;
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Hrw @ " << getHardware().get() << std::endl;
+ for (auto pair: getStateBlockMap())
+ {
+ auto sb = pair.second;
+ _stream << _tabs << " " << pair.first << " sb @ " << sb.get();
+ if (sb)
+ {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+
+ std::stringstream inconsistency_explanation;
+ auto hwd_ptr = getHardware();
+ // check problem and hardware pointers
+ inconsistency_explanation << "Sensor problem pointer " << getProblem().get()
+ << " different from Hardware problem pointer " << hwd_ptr->getProblem().get() << "\n";
+ log.assertTrue((getProblem() == hwd_ptr->getProblem()), inconsistency_explanation);
+
+ inconsistency_explanation << "Sen" << id() << " @ " << _sen_ptr
+ << " ---> Hwd" << " @ " << hwd_ptr
+ << " -X-> Sen" << id();
+ auto hwd_sen_list = hwd_ptr->getSensorList();
+ auto hwd_has_sen = std::find_if(hwd_sen_list.begin(), hwd_sen_list.end(), [&_sen_ptr](SensorBasePtr sen){ return sen == _sen_ptr;});
+ log.assertTrue(hwd_has_sen != hwd_sen_list.end(), inconsistency_explanation);
+
+ for(auto prc : getProcessorList())
+ {
+ inconsistency_explanation << "Sen" << id() << " @ " << _sen_ptr
+ << " ---> Prc" << prc->id() << " @ " << prc
+ << " -X-> Sen" << id();
+ log.assertTrue((prc->getSensor() == _sen_ptr), inconsistency_explanation);
+ }
+ return log;
+}
+
+bool SensorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto sen_ptr = std::static_pointer_cast<SensorBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, sen_ptr, _stream, _tabs);
+ _log.compose(local_log);
+
+ for (auto p : getProcessorList()) p->check(_log, p, _verbose, _stream, _tabs + " ");
+ return _log.is_consistent_;
+}
+
} // namespace wolf
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 84007a6cdbd9afece5ff51b271db25f92d4f41ff..fe1bc2f67a5f79fbf8d45f8b1a1ced7c220ace24 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -148,4 +148,30 @@ void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _stat
F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
+
+CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
+ {
+ _stream << _tabs << "Trj @ " << _trj_ptr.get() << std::endl;
+ }
+
+ // check pointer to Problem
+ inconsistency_explanation << "Trj->getProblem() [" << getProblem().get()
+ << "] -> " << " Prb->getTrajectory() [" << getProblem()->getTrajectory().get() << "] -> Trj [" << _trj_ptr.get() << "] Mismatch!\n";
+ log.assertTrue((_trj_ptr->getProblem()->getTrajectory().get() == _trj_ptr.get()), inconsistency_explanation);
+ return log;
+}
+bool TrajectoryBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto trj_ptr = std::static_pointer_cast<TrajectoryBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, trj_ptr, _stream, _tabs);
+ _log.compose(local_log);
+ for (auto F : getFrameList())
+ F->check(_log, F, _verbose, _stream, _tabs + " ");
+ return _log.is_consistent_;
+}
} // namespace wolf