From 0c90bd21ce6916e0b7c2100bf325df3a904c6a39 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Thu, 16 Apr 2020 16:33:03 +0200
Subject: [PATCH] Make printHeader() more uniform
---
hello_wolf/hello_wolf.cpp | 2 +-
src/capture/capture_base.cpp | 48 +++++++++++++++++++++++++++-------
src/frame/frame_base.cpp | 7 ++---
src/landmark/landmark_base.cpp | 18 +++++++++----
src/sensor/sensor_base.cpp | 1 +
5 files changed, 57 insertions(+), 19 deletions(-)
diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index ac33da83c..80f5288f1 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -260,7 +260,7 @@ int main()
std::cout << std::endl;
WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
- problem->print(4,1,1,1);
+ problem->print(4,0,1,0);
/*
* ============= FIRST COMMENT ON THE RESULTS ==================
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 62e602c54..2175b4ac2 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -297,18 +297,46 @@ void CaptureBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by
}
_stream << std::endl;
- if (_state_blocks)
- for (const auto& sb : getStateBlockVec())
+
+ if (_metric && _state_blocks){
+ for (const auto& key : getStructure())
{
- if(sb != nullptr)
- {
- _stream << " sb: ";
- _stream << (sb->isFixed() ? "Fix" : "Est");
- if (_metric)
- _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
- _stream << std::endl;
- }
+ auto sb = getStateBlock(key);
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
}
+ }
+ else if (_metric)
+ {
+ _stream << (isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
+ << getTimeStamp();
+ _stream << ",\t x = ( " << std::setprecision(2) << getState().transpose() << " )";
+ _stream << std::endl;
+ }
+ else if (_state_blocks)
+ {
+ _stream << " sb:";
+ for (const auto& key : getStructure())
+ {
+ const auto& sb = getStateBlock(key);
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ }
+ _stream << std::endl;
+ }
+
+
+
+// if (_state_blocks)
+// for (const auto& sb : getStateBlockVec())
+// {
+// if(sb != nullptr)
+// {
+// _stream << " sb: ";
+// _stream << (sb->isFixed() ? "Fix" : "Est");
+// if (_metric)
+// _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
+// _stream << std::endl;
+// }
+// }
}
void CaptureBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
{
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index a341335d1..233ea4434 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -374,7 +374,7 @@ void FrameBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by,
for (const auto& key : getStructure())
{
auto sb = getStateBlock(key);
- _stream << " " << key << "," << (sb->isFixed() ? "Fix: " : "Est: ") << sb->getState().transpose() << std::endl;
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
}
}
else if (_metric)
@@ -387,9 +387,10 @@ void FrameBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by,
else if (_state_blocks)
{
_stream << " sb:";
- for (const auto& sb : getStateBlockVec())
+ for (const auto& key : getStructure())
{
- _stream << " " << (sb->isFixed() ? "Fix" : "Est");
+ const auto& sb = getStateBlock(key);
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
}
_stream << std::endl;
}
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 20df53bd0..0750fa7ae 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -163,19 +163,27 @@ void LandmarkBase::printHeader(int _depth, std::ostream& _stream, bool _constr_b
_stream << "Fac" << cby->id() << " \t";
}
_stream << std::endl;
- if (_metric)
+
+ if (_metric && _state_blocks){
+ for (const auto& key : getStructure())
+ {
+ auto sb = getStateBlock(key);
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
+ }
+ }
+ else if (_metric)
{
_stream << (isFixed() ? " Fix" : " Est");
_stream << ",\t x = ( " << std::setprecision(2) << getState().transpose() << " )";
_stream << std::endl;
}
- if (_state_blocks)
+ else if (_state_blocks)
{
_stream << " sb:";
- for (const auto& sb : getStateBlockVec())
+ for (const auto& key : getStructure())
{
- if (sb != nullptr)
- _stream << (sb->isFixed() ? " Fix" : " Est");
+ const auto& sb = getStateBlock(key);
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
}
_stream << std::endl;
}
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 6ed95da19..525202369 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -473,6 +473,7 @@ void SensorBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by,
if (_depth < 2)
_stream << " -- " << getProcessorList().size() << "p";
_stream << std::endl;
+
if (_metric && _state_blocks)
{
_stream << " sb: ";
--
GitLab