Skip to content
Snippets Groups Projects
Commit 98edebff authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Print sensor priors

parent ac3b1fe9
No related branches found
No related tags found
1 merge request!466devel->main
Pipeline #12910 passed
...@@ -551,8 +551,18 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st ...@@ -551,8 +551,18 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st
{ {
auto sb = getStateBlockDynamic(key); auto sb = getStateBlockDynamic(key);
if (sb) 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) else if (_metric)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment