diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index ef0f89d47ff78c447f81cbd9423b32e755f6e8bc..47990c97d0190f0780a475780fc6c7294501a102 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -56,7 +56,6 @@ CaptureBase::CaptureBase(const std::string& _type,
CaptureBase::~CaptureBase()
{
- removeStateBlocks(getProblem());
}
void CaptureBase::remove(bool viral_remove_empty_parent)
@@ -235,8 +234,6 @@ void CaptureBase::setProblem(ProblemPtr _problem)
void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
- _stream << _tabs << "Cap" << id() << " " << getType() << " ts = " << std::setprecision(3) << getTimeStamp();
-
if(getSensor() != nullptr)
{
_stream << " -> Sen" << getSensor()->id();
@@ -261,7 +258,7 @@ void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
{
auto sb = getStateBlock(key);
if (sb)
- _stream << _tabs << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
+ _stream << _tabs << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ) @ " << sb << std::endl;
}
}
else if (_metric)
@@ -277,7 +274,7 @@ void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
{
const auto& sb = getStateBlock(key);
if (sb)
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
}
_stream << std::endl;
}
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index da17a57af80971b21f4a69322ba0c2ade54225d4..bad56dae309cca61a119594e3b396444409e267b 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -108,7 +108,7 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool
_stream << (sb->isFixed() ? "Fix" : "Est");
if (_metric)
_stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
- _stream << std::endl;
+ _stream << " @ " << sb << std::endl;
}
}
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index c2af9127c6a2e028d98f4f5413490117bfbd7825..e057483638369a0621bf2394f8b79053baebe17b 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -177,6 +177,7 @@ void FeatureBase::link(CaptureBasePtr _cpt_ptr)
}
void FeatureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
+ _stream << _tabs << "FeatureBase::printHeader\n";
_stream << _tabs << "Ftr" << id() << " trk" << trackId() << " " << getType() << ((_depth < 4) ? " -- " + std::to_string(getFactorList().size()) + "c " : "");
if (_constr_by)
{
@@ -194,6 +195,8 @@ void FeatureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
void FeatureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
+ _stream << _tabs << "FeatureBase::print\n";
+ _stream << _tabs << "id: " << id() << std::endl;
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 4)
for (auto c : getFactorList())
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 357ee344b6e11df9ceba82304f442e3b3878d16d..20ab9331867cd18963b5bc85b0f42fa1c01b4a2f 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -339,6 +339,7 @@ void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _sta
_stream << _tabs << " " << key
<< "[" << (sb->isFixed() ? "Fix" : "Est")
<< "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )"
+ << " @ " << sb
<< std::endl;
}
}
@@ -355,7 +356,7 @@ void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _sta
{
const auto& sb = getStateBlock(key);
if (sb)
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
}
_stream << std::endl;
}
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 555f74f912d91e4687eda0a07aae96960e2ce2bf..c8f855dfb512a8e1438f07c0c9582db8d2c0f578 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -175,7 +175,7 @@ void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _
{
auto sb = getStateBlock(key);
if (sb)
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ) @ " << sb << std::endl;
}
}
else if (_metric)
@@ -191,7 +191,7 @@ void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _
{
const auto& sb = getStateBlock(key);
if (sb)
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
}
_stream << std::endl;
}
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index b1742c5aea60265d255aa142733ae45961d16e94..82fbe684558bd9791763b70c0bbedab68514b8f0 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -480,9 +480,7 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
{
auto sb = getStateBlockDynamic(key);
if (sb)
- _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
- else
- _stream << "NULLPTR ";
+ _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ) @ " << sb << " ";
}
_stream << std::endl;
}
@@ -494,8 +492,6 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
auto sb = getStateBlockDynamic(key);
if (sb)
_stream << sb->getState().transpose() << " ";
- else
- _stream << "NULLPTR ";
}
_stream << ")" << std::endl;
}
@@ -506,9 +502,7 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
{
auto sb = getStateBlockDynamic(key);
if (sb)
- _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
- else
- _stream << "NULLPTR ";
+ _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb << " ";
}
_stream << std::endl;
}