diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 94df0d2c9d77aba7e61ee7f70b87aafc927d6fa6..a8d5f502eed347c722321651b96fe0dbf1501719 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -247,7 +247,8 @@ void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
{
_stream << " <-- ";
for (auto cby : getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
+ if (cby)
+ _stream << "Fac" << cby->id() << " \t";
}
_stream << std::endl;
@@ -257,7 +258,8 @@ void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
for (const auto& key : getStructure())
{
auto sb = getStateBlock(key);
- _stream << _tabs << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
+ if (sb)
+ _stream << _tabs << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
}
}
else if (_metric)
@@ -272,7 +274,8 @@ void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
for (const auto& key : getStructure())
{
const auto& sb = getStateBlock(key);
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ if (sb)
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
}
_stream << std::endl;
}
@@ -284,7 +287,8 @@ void CaptureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 3)
for (auto f : getFeatureList())
- f->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+ if (f)
+ 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
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 69133345e0f57c28e3135aa3b50e0d99e179be50..c2af9127c6a2e028d98f4f5413490117bfbd7825 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -182,7 +182,8 @@ void FeatureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
{
_stream << " <--\t";
for (auto cby : getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
+ if (cby)
+ _stream << "Fac" << cby->id() << " \t";
}
_stream << std::endl;
if (_metric)
@@ -196,7 +197,8 @@ void FeatureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 4)
for (auto c : getFactorList())
- c->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+ if (c)
+ 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
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index f7c7ce6ea257a48f73c68003b8c1efbfeb4f6a6b..80b804d9013d8af5526f95c7e0f17e80c929cc2b 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -324,7 +324,8 @@ void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _sta
{
_stream << " <-- ";
for (auto cby : getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
+ if (cby)
+ _stream << "Fac" << cby->id() << " \t";
}
_stream << std::endl;
@@ -333,10 +334,11 @@ void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _sta
for (const auto& key : getStructure())
{
auto sb = getStateBlock(key);
- _stream << _tabs << " " << key
- << "[" << (sb->isFixed() ? "Fix" : "Est")
- << "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )"
- << std::endl;
+ if (sb)
+ _stream << _tabs << " " << key
+ << "[" << (sb->isFixed() ? "Fix" : "Est")
+ << "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )"
+ << std::endl;
}
}
else if (_metric)
@@ -351,7 +353,8 @@ void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _sta
for (const auto& key : getStructure())
{
const auto& sb = getStateBlock(key);
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ if (sb)
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
}
_stream << std::endl;
}
@@ -362,7 +365,8 @@ void FrameBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blo
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 2)
for (auto C : getCaptureList())
- C->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+ if (C)
+ 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
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index 72f07b8e216e6ca830ec9021dbb7ca1144d09860..e6e5a15c3c216d2abfa9242a48bb97081e22d016 100644
--- a/src/hardware/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -30,7 +30,8 @@ void HardwareBase::print(int _depth, bool _constr_by, bool _metric, bool _state_
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 1)
for (auto S : getSensorList())
- S->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+ if (S)
+ 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
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index af2d01b68e40c33b129f799d93fe3d133459671b..555f74f912d91e4687eda0a07aae96960e2ce2bf 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -165,7 +165,8 @@ void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _
{
_stream << "\t<-- ";
for (auto cby : getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
+ if (cby)
+ _stream << "Fac" << cby->id() << " \t";
}
_stream << std::endl;
@@ -173,7 +174,8 @@ void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _
for (const auto& key : getStructure())
{
auto sb = getStateBlock(key);
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
+ if (sb)
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
}
}
else if (_metric)
@@ -188,7 +190,8 @@ void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _
for (const auto& key : getStructure())
{
const auto& sb = getStateBlock(key);
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ if (sb)
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
}
_stream << std::endl;
}
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 67c37971b4e0517017f03b9cd798080fcd40808d..7f6db136f0d1644bb4d2ff193fc3c7d776fdfa26 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -99,7 +99,8 @@ void MapBase::print(int _depth, bool _constr_by, bool _metric, bool _state_block
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 1)
for (auto L : getLandmarkList())
- L->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+ if (L)
+ 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
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 5c769cde325442b711ff36c8a18f557a289ed6f6..caa850ebcd7b3b6d9e443c05266cc1e1d67491c0 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -220,7 +220,6 @@ void BufferPackKeyFrame::print(void) const
void ProcessorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
_stream << _tabs << "Prc" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
-
}
void ProcessorBase::print(int _depth, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index fefc4d332eca061b041585bf42c6248fb985d992..696061983077c6da19fe292206bff168bacc4b95 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -479,7 +479,8 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
for (auto& key : getStructure())
{
auto sb = getStateBlockDynamic(key);
- _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
+ if (sb)
+ _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
}
_stream << std::endl;
}
@@ -489,7 +490,8 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
for (auto& key : getStructure())
{
auto sb = getStateBlockDynamic(key);
- _stream << sb->getState().transpose() << " ";
+ if (sb)
+ _stream << sb->getState().transpose() << " ";
}
_stream << ")" << std::endl;
}
@@ -499,7 +501,8 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
for (auto& key : getStructure())
{
auto sb = getStateBlockDynamic(key);
- _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ if (sb)
+ _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
}
_stream << std::endl;
}
@@ -511,7 +514,8 @@ void SensorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_bl
if (_depth >= 2)
for (auto p : getProcessorList())
- p->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+ if (p)
+ 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
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 8ab61006c9e46acfdeee6ad735257bdf9d776ff7..23f967716a28e65d9aa09a01b1cb60a345f220e5 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -91,8 +91,8 @@ void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _stat
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 1)
for (auto F : *this)
- F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
-
+ if (F)
+ 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