Merge branch 'devel' into 213-std-set-and-std-map-instead-of-std-list-in-wolf-nodes

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;
 

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 {
 

include/core/factor/factor_base.h

@@ -222,6 +222,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;}

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);

include/core/frame/frame_base.h

@@ -136,6 +136,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);

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);
 };

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);

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();
 };

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

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>

include/core/trajectory/trajectory_base.h

@@ -93,6 +93,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);

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

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

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

src/frame/frame_base.cpp

@@ -358,4 +358,106 @@ 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 << (isKey() ? "KFrm" : "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_has_frm = std::find_if(trj_ptr->begin(), trj_ptr->end(), [&_frm_ptr](FrameBasePtr frm){ return frm == _frm_ptr;});
+    log.assertTrue(trj_has_frm != trj_ptr->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

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

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
 {

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

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