From 075927963abf16de9f166d8f484ca3a9e47df497 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Thu, 30 Apr 2020 16:25:10 +0200
Subject: [PATCH] Rework check() 2on iteration
---
src/capture/capture_base.cpp | 16 ++++++++++++++++
src/factor/factor_base.cpp | 7 +++++++
src/feature/feature_base.cpp | 16 ++++++++++++++++
src/frame/frame_base.cpp | 16 ++++++++++++++++
src/landmark/landmark_base.cpp | 8 ++++++++
src/processor/processor_base.cpp | 7 +++++++
src/sensor/sensor_base.cpp | 14 ++++++++++++++
7 files changed, 84 insertions(+)
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 4957daf4c..f596b4c1e 100644
--- a/src/capture/capture_base.cpp
+++ b/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
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 698525ac0..919208234 100644
--- a/src/factor/factor_base.cpp
+++ b/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();
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 6dc625c79..0acccaf7a 100644
--- a/src/feature/feature_base.cpp
+++ b/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
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 27bead7e2..bca65f5a4 100644
--- a/src/frame/frame_base.cpp
+++ b/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;
}
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 2c4db3060..9f2552615 100644
--- a/src/landmark/landmark_base.cpp
+++ b/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
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 230a4c935..ada805258 100644
--- a/src/processor/processor_base.cpp
+++ b/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
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index fe5891064..2ae05abfd 100644
--- a/src/sensor/sensor_base.cpp
+++ b/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;
}
--
GitLab