Start integrating GNSS observation. Added a new CLI program mola-navs…

…tate-cli for testing state fusion

mola_navstate_fg/CMakeLists.txt

@@ -45,6 +45,15 @@ mola_add_library(
 )
 target_include_directories(${PROJECT_NAME} PRIVATE ".")
 
+# -----------------------
+# define cli apps:
+mola_add_executable(
+  TARGET  mola-navstate-cli
+  SOURCES apps/mola-navstate-cli.cpp
+  LINK_LIBRARIES
+    ${PROJECT_NAME}
+)
+
 # -----------------------
 # define tests:
 enable_testing()

mola_navstate_fg/apps/mola-navstate-cli.cpp

@@ -0,0 +1,93 @@
+/* -------------------------------------------------------------------------
+ *   A Modular Optimization framework for Localization and mApping  (MOLA)
+ * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
+ * See LICENSE for license information.
+ * ------------------------------------------------------------------------- */
+
+#include <mola_navstate_fg/NavStateFG.h>
+#include <mrpt/core/exceptions.h>
+#include <mrpt/obs/CObservationGPS.h>
+#include <mrpt/obs/CObservationIMU.h>
+#include <mrpt/obs/CObservationRobotPose.h>
+#include <mrpt/obs/CRawlog.h>
+
+#include <iostream>
+
+namespace
+{
+void run_navstate(const std::string& paramsFile, const std::string& rawlogFile)
+{
+    using mrpt::obs::CObservationGPS;
+    using mrpt::obs::CObservationIMU;
+    using mrpt::obs::CObservationRobotPose;
+
+    mola::NavStateFG nav;
+
+    std::cout << "Initalizing from: " << paramsFile << std::endl;
+
+    const auto cfg = mrpt::containers::yaml::FromFile(paramsFile);
+    nav.initialize(cfg);
+
+    std::cout << "Reading dataset from: " << rawlogFile << std::endl;
+
+    mrpt::obs::CRawlog dataset;
+    dataset.loadFromRawLogFile(rawlogFile);
+
+    const std::string frame_id = "map";
+
+    std::cout << "Read entries: " << dataset.size() << std::endl;
+
+    for (size_t i = 0; i < dataset.size(); i++)
+    {
+        const auto o = dataset.getAsObservation(i);
+        if (!o) continue;
+
+        if (auto oGPS = std::dynamic_pointer_cast<CObservationGPS>(o); oGPS)
+        {
+            nav.fuse_gnss(*oGPS);
+        }
+        else if (auto oPose =
+                     std::dynamic_pointer_cast<CObservationRobotPose>(o);
+                 oPose)
+        {
+            nav.fuse_pose(oPose->timestamp, oPose->pose, frame_id);
+        }
+        else if (auto oImu = std::dynamic_pointer_cast<CObservationIMU>(o);
+                 oImu)
+        {
+            nav.fuse_imu(*oImu);
+        }
+        else
+        {
+            std::cout << "[Warning] Ignoring observation #" << i << ": '"
+                      << o->sensorLabel
+                      << "' of type: " << o->GetRuntimeClass()->className
+                      << "\n";
+        }
+
+    }  // for each entry
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+    try
+    {
+        if (argc != 3)
+        {
+            std::cerr << "Usage: " << argv[0] << " params.yaml dataset.rawlog"
+                      << std::endl;
+            return 1;
+        }
+
+        run_navstate(argv[1], argv[2]);
+
+        return 0;
+    }
+    catch (const std::exception& e)
+    {
+        std::cerr << "Exception: " << e.what() << std::endl;
+        return 1;
+    }
+}

mola_navstate_fg/include/mola_navstate_fg/NavStateFG.h

@@ -128,6 +128,9 @@ class NavStateFG : public mola::NavStateFilter
     /** Integrates new IMU observations into the estimator */
     void fuse_imu(const mrpt::obs::CObservationIMU& imu) override;
 
+    /** Integrates new GNSS observations into the estimator */
+    void fuse_gnss(const mrpt::obs::CObservationGPS& gps) override;
+
     /** Integrates new twist estimation (in the odom frame) */
     void fuse_twist(
         const mrpt::Clock::time_point&     timestamp,

mola_navstate_fg/src/NavStateFG.cpp

@@ -154,6 +154,14 @@ void NavStateFG::fuse_imu(const mrpt::obs::CObservationIMU& imu)
     delete_too_old_entries();
 }
 
+void NavStateFG::fuse_gnss(const mrpt::obs::CObservationGPS& gps)
+{
+    THROW_EXCEPTION("TODO");
+    (void)gps;
+
+    delete_too_old_entries();
+}
+
 void NavStateFG::fuse_pose(
     const mrpt::Clock::time_point&         timestamp,
     const mrpt::poses::CPose3DPDFGaussian& pose, const std::string& frame_id)

mola_navstate_fg/tests/mola-navstate-fg-params.yaml

@@ -0,0 +1,10 @@
+# Config for NavStateFGParams
+sliding_window_length: 5.0 # [s]
+max_time_to_use_velocity_model: 2.0  # [s]
+time_between_frames_to_warning: 2.0  # [s]
+sigma_random_walk_acceleration_linear: 1.0 # [m/s²]
+sigma_random_walk_acceleration_angular: 1.0 # [rad/s²]
+sigma_integrator_position: 0.10 # [m]
+sigma_integrator_orientation: 0.10 # [rad]
+robust_param: 0
+max_rmse: 2

mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h

@@ -98,6 +98,9 @@ class NavStateFuse : public mola::NavStateFilter
     /** Integrates new IMU observations into the estimator */
     void fuse_imu(const mrpt::obs::CObservationIMU& imu) override;
 
+    /** Integrates new GNSS observations into the estimator */
+    void fuse_gnss(const mrpt::obs::CObservationGPS& gps) override;
+
     /** Integrates new twist estimation (in the odom frame) */
     void fuse_twist(
         const mrpt::Clock::time_point&     timestamp,

mola_navstate_fuse/src/NavStateFuse.cpp

@@ -70,6 +70,12 @@ void NavStateFuse::fuse_imu(const mrpt::obs::CObservationIMU& imu)
     (void)imu;
 }
 
+void NavStateFuse::fuse_gnss(const mrpt::obs::CObservationGPS& gps)
+{
+    // TODO(jlbc)
+    (void)gps;
+}
+
 void NavStateFuse::fuse_pose(
     const mrpt::Clock::time_point&         timestamp,
     const mrpt::poses::CPose3DPDFGaussian& pose,