diff --git a/mola_input_mulran_dataset/src/MulranDataset.cpp b/mola_input_mulran_dataset/src/MulranDataset.cpp index 8a8b41c3..8ccc7a23 100644 --- a/mola_input_mulran_dataset/src/MulranDataset.cpp +++ b/mola_input_mulran_dataset/src/MulranDataset.cpp @@ -598,6 +598,12 @@ mrpt::obs::CObservationGPS::Ptr MulranDataset::get_gps_by_row_index( obs->messages[mrpt::obs::gnss::NMEA_GGA] = msg; + // full 3x3 cov: + auto& cov = obs->covariance_enu.emplace(); + for (int r = 0, i = 0; r < 3; r++) + for (int c = 0; c < 3; c++) // + cov(r, c) = gpsCsvData_(row, 4 + (i++)); + return obs; } diff --git a/mola_viz/src/MolaViz.cpp b/mola_viz/src/MolaViz.cpp index bc4ee0ec..6614d2ce 100644 --- a/mola_viz/src/MolaViz.cpp +++ b/mola_viz/src/MolaViz.cpp @@ -435,7 +435,7 @@ void gui_handler_gps( auto obj = std::dynamic_pointer_cast(o); if (!obj) return; - std::array labels; + std::array labels; labels.fill(nullptr); if (w->children().size() == 1) { @@ -475,6 +475,16 @@ void gui_handler_gps( static_cast(gga->fields.UTCTime.minute), gga->fields.UTCTime.sec)); } + if (obj->covariance_enu.has_value()) + { + const auto& cov = obj->covariance_enu.value(); + const double std_x = std::sqrt(cov(0, 0)); + const double std_y = std::sqrt(cov(1, 1)); + const double std_z = std::sqrt(cov(2, 2)); + + labels[5]->setCaption(mrpt::format( + "sigmas [m]: x=%.02f y=%.02f z=%.02f", std_x, std_y, std_z)); + } } } // namespace