use new mrpt GPS covariance field

MOLAorg · Mar 9, 2024 · 3f141f4 · 3f141f4
1 parent 865626a
commit 3f141f4
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 1 deletion.
diff --git 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
@@ -435,7 +435,7 @@ void gui_handler_gps(
     auto obj = std::dynamic_pointer_cast<mrpt::obs::CObservationGPS>(o);
     if (!obj) return;
 
-    std::array<nanogui::Label*, 5> labels;
+    std::array<nanogui::Label*, 6> labels;
     labels.fill(nullptr);
     if (w->children().size() == 1)
     {
@@ -475,6 +475,16 @@ void gui_handler_gps(
             static_cast<unsigned int>(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