From c22392177f604d949a87b601f89a8dc9cef67da1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Tue, 6 Oct 2020 13:02:09 +0200
Subject: [PATCH] print(): add @ of state blocks
---
src/capture/capture_base.cpp | 7 ++-----
src/capture/capture_motion.cpp | 2 +-
src/feature/feature_base.cpp | 3 +++
src/frame/frame_base.cpp | 3 ++-
src/landmark/landmark_base.cpp | 4 ++--
src/sensor/sensor_base.cpp | 10 ++--------
6 files changed, 12 insertions(+), 17 deletions(-)
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index ef0f89d47..47990c97d 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 da17a57af..bad56dae3 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 c2af9127c..e05748363 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 357ee344b..20ab93318 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 555f74f91..c8f855dfb 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 b1742c5ae..82fbe6845 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;
}
--
GitLab