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