From 98edebff5d28d9d83d07fb071ae2fbd3b2f50b7a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Tue, 23 Aug 2022 13:21:09 +0200
Subject: [PATCH] Print sensor priors

---
 src/sensor/sensor_base.cpp | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 0027844ea..af06fd136 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -551,8 +551,18 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st
         {
             auto sb = getStateBlockDynamic(key);
             if (sb)
-                _stream << _tabs << "  " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << ", " << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "] = ( "
-                        << std::setprecision(3) << sb->getState().transpose() << " )" << " @ " << sb << std::endl;
+            {
+                _stream << _tabs << "  " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << ", "
+                        << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "] = ( " << std::setprecision(3)
+                        << sb->getState().transpose() << " )";
+                if (params_prior_map_.count(key))
+                {
+                    auto ftr = params_prior_map_.at(key);
+                    _stream << " ; mean = ( " << ftr->getMeasurement().transpose() << " )";
+                    _stream << " , std = ( " << ftr->getMeasurementCovariance().diagonal().array().sqrt().transpose() << " )";
+                }
+                _stream << " @ " << sb << std::endl;
+            }
         }
     }
     else if (_metric)
-- 
GitLab