Rework check() 2on iteration

src/capture/capture_base.cpp

@@ -421,6 +421,22 @@ CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::os
             }
         }
     }
+
+        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

src/factor/factor_base.cpp

@@ -454,6 +454,13 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostr
     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();

src/feature/feature_base.cpp

@@ -226,6 +226,22 @@ CheckLog FeatureBase::localCheck(bool _verbose, FeatureBasePtr _ftr_ptr, std::os
         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

src/frame/frame_base.cpp

@@ -478,6 +478,22 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
             }
         }
     }
+
+    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;
 }
 

src/landmark/landmark_base.cpp

@@ -285,6 +285,14 @@ CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::
             }
         }
     }
+
+    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

src/processor/processor_base.cpp

@@ -228,6 +228,13 @@ CheckLog ProcessorBase::localCheck(bool _verbose, ProcessorBasePtr _prc_ptr, std
                               << " 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

src/sensor/sensor_base.cpp

@@ -547,6 +547,20 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostr
                                   << " 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;
 }