Skip to content
Snippets Groups Projects
Commit 9b07c944 authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Rework check() 1st iteration

Reworked Problem::check() so that each node is responsible for checking
their own consistency
parent 5a44f964
No related branches found
No related tags found
1 merge request!361WIP: Resolve "Rework Problem::check into a recursive approach"
Pipeline #5278 failed
This commit is part of merge request !361. Comments created here will be created in the context of that merge request.
Showing
with 683 additions and 608 deletions
...@@ -116,6 +116,9 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s ...@@ -116,6 +116,9 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
std::ostream& stream = std::cout, std::ostream& stream = std::cout,
std::string _tabs = "") const; 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: protected:
virtual SizeEigen computeCalibSize() const; virtual SizeEigen computeCalibSize() const;
......
...@@ -3,6 +3,7 @@ ...@@ -3,6 +3,7 @@
// Wolf includes // Wolf includes
#include "core/common/wolf.h" #include "core/common/wolf.h"
#include "core/utils/check_log.h"
namespace wolf { namespace wolf {
......
...@@ -220,6 +220,9 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa ...@@ -220,6 +220,9 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
std::ostream& stream = std::cout, std::ostream& stream = std::cout,
std::string _tabs = "") const; 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: private:
void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;} void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
......
...@@ -119,6 +119,9 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature ...@@ -119,6 +119,9 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
std::ostream& stream = std::cout, std::ostream& stream = std::cout,
std::string _tabs = "") const; 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: private:
void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;} void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
FactorBasePtr addFactor(FactorBasePtr _co_ptr); FactorBasePtr addFactor(FactorBasePtr _co_ptr);
......
...@@ -144,6 +144,10 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha ...@@ -144,6 +144,10 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
bool state_blocks, bool state_blocks,
std::ostream& stream = std::cout, std::ostream& stream = std::cout,
std::string _tabs = "") const; 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: private:
CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr); CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
......
...@@ -40,6 +40,9 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa ...@@ -40,6 +40,9 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
std::ostream& stream = std::cout, std::ostream& stream = std::cout,
std::string _tabs = "") const; 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: private:
virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr); virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
}; };
......
...@@ -99,6 +99,9 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_ ...@@ -99,6 +99,9 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
std::ostream& stream = std::cout, std::ostream& stream = std::cout,
std::string _tabs = "") const; 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: private:
void setMap(const MapBasePtr _map_ptr); void setMap(const MapBasePtr _map_ptr);
......
...@@ -50,6 +50,9 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase> ...@@ -50,6 +50,9 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
bool state_blocks, bool state_blocks,
std::ostream& stream = std::cout, std::ostream& stream = std::cout,
std::string _tabs = "") const; 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: private:
std::string dateTimeNow(); std::string dateTimeNow();
}; };
......
...@@ -375,6 +375,9 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce ...@@ -375,6 +375,9 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
bool state_blocks, bool state_blocks,
std::ostream& stream = std::cout, std::ostream& stream = std::cout,
std::string _tabs = "") const; 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 inline bool ProcessorBase::isVotingActive() const
......
...@@ -255,6 +255,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh ...@@ -255,6 +255,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
std::ostream& stream = std::cout, std::ostream& stream = std::cout,
std::string _tabs = "") const; 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); void link(HardwareBasePtr);
template<typename classType, typename... T> template<typename classType, typename... T>
......
...@@ -59,6 +59,9 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj ...@@ -59,6 +59,9 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
bool state_blocks, bool state_blocks,
std::ostream& stream = std::cout, std::ostream& stream = std::cout,
std::string _tabs = "") const; 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: private:
FrameBasePtr addFrame(FrameBasePtr _frame_ptr); FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
void removeFrame(FrameBasePtr _frame_ptr); void removeFrame(FrameBasePtr _frame_ptr);
......
...@@ -347,4 +347,89 @@ void CaptureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b ...@@ -347,4 +347,89 @@ void CaptureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b
for (auto f : getFeatureList()) for (auto f : getFeatureList())
f->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); 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;
}
}
}
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 } // namespace wolf
...@@ -327,4 +327,277 @@ void FactorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_bl ...@@ -327,4 +327,277 @@ void FactorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_bl
{ {
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs); 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);
// 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 } // namespace wolf
...@@ -188,4 +188,54 @@ void FeatureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b ...@@ -188,4 +188,54 @@ void FeatureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b
for (auto c : getFactorList()) for (auto c : getFactorList())
c->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); 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);
}
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 } // namespace wolf
...@@ -403,4 +403,91 @@ void FrameBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blo ...@@ -403,4 +403,91 @@ void FrameBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blo
for (auto C : getCaptureList()) for (auto C : getCaptureList())
C->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); 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;
}
}
}
}
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 } // namespace wolf
...@@ -32,4 +32,29 @@ void HardwareBase::print(int _depth, bool _constr_by, bool _metric, bool _state_ ...@@ -32,4 +32,29 @@ void HardwareBase::print(int _depth, bool _constr_by, bool _metric, bool _state_
for (auto S : getSensorList()) for (auto S : getSensorList())
S->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); 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 } // namespace wolf
...@@ -219,6 +219,82 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node) ...@@ -219,6 +219,82 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
return lmk; 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;
}
}
}
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 // Register landmark creator
namespace namespace
{ {
......
...@@ -101,4 +101,28 @@ void MapBase::print(int _depth, bool _constr_by, bool _metric, bool _state_block ...@@ -101,4 +101,28 @@ void MapBase::print(int _depth, bool _constr_by, bool _metric, bool _state_block
for (auto L : getLandmarkList()) for (auto L : getLandmarkList())
L->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " "); 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 } // namespace wolf
This diff is collapsed.
...@@ -210,4 +210,31 @@ void ProcessorBase::print(int _depth, bool _metric, bool _state_blocks, bool _co ...@@ -210,4 +210,31 @@ void ProcessorBase::print(int _depth, bool _metric, bool _state_blocks, bool _co
{ {
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs); 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);
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 } // namespace wolf
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment