New state estimators (Merge after MOLA 1.5.0 is installable via apt)

.github/workflows/build-ros.yml

@@ -22,17 +22,29 @@ jobs:
           # Humble Hawksbill (May 2022 - May 2027)
           - docker_image: ubuntu:jammy
             ros_distribution: humble
-            ros_version: 2
+            use_ros_testing: false
+
+          - docker_image: ubuntu:jammy
+            ros_distribution: humble
+            use_ros_testing: true
 
           # Jazzy (2024 - ??)
           - docker_image: ubuntu:noble
             ros_distribution: jazzy
-            ros_version: 2
+            use_ros_testing: false
+
+          - docker_image: ubuntu:noble
+            ros_distribution: jazzy
+            use_ros_testing: true
 
           # Rolling Ridley (No End-Of-Life)
           - docker_image: ubuntu:noble
             ros_distribution: rolling
-            ros_version: 2
+            use_ros_testing: false
+
+          - docker_image: ubuntu:noble
+            ros_distribution: rolling
+            use_ros_testing: true
 
     container:
       image: ${{ matrix.docker_image }}
@@ -51,7 +63,7 @@ jobs:
         uses: ros-tooling/[email protected]
         with:
           required-ros-distributions: ${{ matrix.ros_distribution }}
-          use-ros2-testing: true
+          use-ros2-testing: ${{ matrix.use_ros_testing }}
 
       - name: Install rosdep dependencies
         run: |

CMakeLists.txt

@@ -26,7 +26,6 @@ find_package(mrpt-tclap REQUIRED)
 find_mola_package(mola_kernel)
 find_mola_package(mp2p_icp)
 find_mola_package(mp2p_icp_filters)
-find_mola_package(mola_navstate_fuse)
 find_mola_package(mola_pose_list)
 
 # -----------------------
@@ -42,15 +41,13 @@ mola_add_library(
     mola::mola_kernel
     mola::mp2p_icp
     mola::mp2p_icp_filters
-    mola::mola_navstate_fuse
     mola::mola_pose_list
 # PRIVATE_LINK_LIBRARIES
 #  mrpt::maps
   CMAKE_DEPENDENCIES
     mola_kernel
     mp2p_icp
     mp2p_icp_filters
-    mola_navstate_fuse
     mrpt-maps
 )
 
@@ -138,6 +135,7 @@ install(
     pipelines
     ros2-launchs
     rviz2
+    state-estimator-params
   DESTINATION
     share/${PROJECT_NAME}
 )

docs/mola_lo_pipelines.rst

@@ -296,7 +296,7 @@ These settings only have effects if launched via :ref:`MOLA-LO GUI applications
 
 Motion model
 ^^^^^^^^^^^^^^^^^^^^^^
-A constant velocity motion model is used by default, provided by the ``mola_navstate_fuse`` module.
+A constant velocity motion model is used by default, provided by the ``mola_state_estimation_simple`` module.
 
 - ``MOLA_MAX_TIME_TO_USE_VELOCITY_MODEL`` (Default: 0.75 s): Maximum time between LiDAR frames to use the velocity model. Larger delays will cause using the latest vehicle pose as initial guess.
 - ``MOLA_NAVSTATE_SIGMA_RANDOM_WALK_LINACC`` (Default: 1.0 m/s²): Linear acceleration standard deviation.

module/include/mola_lidar_odometry/LidarOdometry.h

@@ -34,12 +34,11 @@
 #include <mola_kernel/interfaces/LocalizationSourceBase.h>
 #include <mola_kernel/interfaces/MapServer.h>
 #include <mola_kernel/interfaces/MapSourceBase.h>
+#include <mola_kernel/interfaces/NavStateFilter.h>
 #include <mola_kernel/interfaces/Relocalization.h>
 #include <mola_kernel/version.h>
 
 // Other packages:
-#include <mola_navstate_fuse/NavStateFuse.h>
-#include <mola_navstate_fuse/NavStateFuseParams.h>
 #include <mola_pose_list/SearchablePoseList.h>
 
 // MP2P_ICP
@@ -67,6 +66,13 @@
 #include <string>
 #include <vector>
 
+// Fwrd decls:
+namespace nanogui
+{
+class Label;
+class CheckBox;
+}  // namespace nanogui
+
 namespace mola
 {
 /** LIDAR-inertial odometry based on ICP against a local metric map model.
@@ -277,8 +283,6 @@ class LidarOdometry : public mola::FrontEndBase,
 
     std::map<AlignKind, ICP_case> icp;
 
-    mola::NavStateFuseParams navstate_fuse_params;
-
     // === SIMPLEMAP GENERATION ====
     struct SimpleMapOptions
     {
@@ -490,27 +494,30 @@ class LidarOdometry : public mola::FrontEndBase,
   {
     using Ptr = std::shared_ptr<ICP_Input>;
 
-    AlignKind align_kind = AlignKind::RegularOdometry;
+    mrpt::poses::CPose3D last_keyframe_pose;
+    std::optional<mrpt::poses::CPose3DPDFGaussianInf> prior;
     id_t global_id = mola::INVALID_ID;
     id_t local_id = mola::INVALID_ID;
+    double time_since_last_keyframe = 0;
     mp2p_icp::metric_map_t::Ptr global_pc, local_pc;
     mrpt::math::TPose3D init_guess_local_wrt_global;
     mp2p_icp::Parameters icp_params;
-    mrpt::poses::CPose3D last_keyframe_pose;
-    double time_since_last_keyframe = 0;
-
-    std::optional<mrpt::poses::CPose3DPDFGaussianInf> prior;
+    AlignKind align_kind = AlignKind::RegularOdometry;
   };
   struct ICP_Output
   {
-    double goodness = .0;
+    ICP_Output() = default;
+
     mrpt::poses::CPose3DPDFGaussian found_pose_to_wrt_from;
+    double goodness = .0;
     uint32_t icp_iterations = 0;
   };
 
   /** All variables that hold the algorithm state */
   struct MethodState
   {
+    MethodState() = default;
+
     bool initialized = false;
     bool fatal_error = false;
 
@@ -537,7 +544,8 @@ class LidarOdometry : public mola::FrontEndBase,
     std::map<std::string /*label*/, mrpt::obs::CObservation::Ptr> sync_obs;
 
     // navstate_fuse to merge pose estimates, IMU, odom, estimate twist.
-    mola::NavStateFuse navstate_fuse;
+    std::shared_ptr<mola::NavStateFilter> navstate_fuse;
+
     std::optional<NavState> last_motion_model_output;
 
     /// The source of "dynamic variables" in ICP pipelines:

module/src/LidarOdometry.cpp

@@ -47,6 +47,7 @@
 #include <mrpt/maps/CColouredPointsMap.h>
 #include <mrpt/obs/CObservation2DRangeScan.h>
 #include <mrpt/obs/CObservationGPS.h>
+#include <mrpt/obs/CObservationIMU.h>
 #include <mrpt/obs/CObservationOdometry.h>
 #include <mrpt/obs/CObservationPointCloud.h>
 #include <mrpt/obs/CRawlog.h>
@@ -63,17 +64,13 @@
 #include <mrpt/system/filesystem.h>
 #include <mrpt/system/string_utils.h>
 
+// GUI:
+#include <mrpt/gui/CDisplayWindowGUI.h>
+
 // STD:
 #include <chrono>
 #include <thread>
 
-// fix for older mrpt versions:
-#include <mrpt/config.h>
-#if MRPT_VERSION <= 0x020d00  // < v2.13.0
-// YAML API:
-#define asSequenceRange asSequence
-#endif
-
 using namespace mola;
 
 // arguments: class_name, parent_class, class namespace
@@ -336,9 +333,12 @@ void LidarOdometry::initialize_frontend(const Yaml & c)
   if (c.has("initial_localization"))
     params_.initial_localization.initialize(c["initial_localization"]);
 
-  ENSURE_YAML_ENTRY_EXISTS(c, "navstate_fuse_params");
-  state_.navstate_fuse.setMinLoggingLevel(this->getMinLoggingLevel());
-  state_.navstate_fuse.initialize(c["navstate_fuse_params"]);
+  // Watch for legacy (mola_lidar_odometry version <0.5.0) organization:
+  if (c.has("navstate_fuse_params")) {
+    THROW_EXCEPTION(
+      "It seems you are using a legacy mola_lo pipeline config file. Please, refer to release "
+      "notes for mola_lidar_odometry 0.5.0");
+  }
 
   ENSURE_YAML_ENTRY_EXISTS(c, "icp_settings_with_vel");
   load_icp_set_of_params(params_.icp[AlignKind::RegularOdometry], c["icp_settings_with_vel"]);
@@ -478,6 +478,19 @@ void LidarOdometry::initialize_frontend(const Yaml & c)
     ASSERT_(loadOk);
   }
 
+  // Attach to the state estimation module, which since MOLA-LO v0.5.0,
+  // must run as a separate MOLA module:
+  {
+    auto mods = findService<mola::NavStateFilter>();
+    ASSERTMSG_(
+      mods.size() == 1,
+      "No state estimation MOLA module (mola::NavStateFilter) was found. Please, check your MOLA "
+      "system .yaml file");
+    state_.navstate_fuse = std::dynamic_pointer_cast<NavStateFilter>(mods[0]);
+    ASSERT_(state_.navstate_fuse);
+    MRPT_LOG_DEBUG("Attached to the state estimation module");
+  }
+
   // end of initialization:
   state_.initialized = true;
   state_.active = params_.start_active;
@@ -799,13 +812,14 @@ void LidarOdometry::onLidarImpl(const CObservation::Ptr & obs)
       initPose.cov = *il.initial_pose_cov;
     }
 
-    state_.navstate_fuse.reset();  // needed after a re-localization to forget the past
+    ASSERT_(state_.navstate_fuse);
+    state_.navstate_fuse->reset();  // needed after a re-localization to forget the past
 
     // Fake an evolution to be able to have an initial velocity estimation:
     const auto t1 = mrpt::Clock::fromDouble(mrpt::Clock::toDouble(this_obs_tim) - 0.2);
     const auto t2 = mrpt::Clock::fromDouble(mrpt::Clock::toDouble(this_obs_tim) - 0.1);
-    state_.navstate_fuse.fuse_pose(t1, initPose, NAVSTATE_LIODOM_FRAME);
-    state_.navstate_fuse.fuse_pose(t2, initPose, NAVSTATE_LIODOM_FRAME);
+    state_.navstate_fuse->fuse_pose(t1, initPose, NAVSTATE_LIODOM_FRAME);
+    state_.navstate_fuse->fuse_pose(t2, initPose, NAVSTATE_LIODOM_FRAME);
 
     MRPT_LOG_INFO_STREAM("Initial re-localization done with pose: " << initPose.mean);
 
@@ -827,7 +841,7 @@ void LidarOdometry::onLidarImpl(const CObservation::Ptr & obs)
   ProfilerEntry tleMotion(profiler_, "onLidar.2b.estimated_navstate");
 
   state_.last_motion_model_output =
-    state_.navstate_fuse.estimated_navstate(this_obs_tim, NAVSTATE_LIODOM_FRAME);
+    state_.navstate_fuse->estimated_navstate(this_obs_tim, NAVSTATE_LIODOM_FRAME);
 
   const bool hasMotionModel = state_.last_motion_model_output.has_value();
 
@@ -854,7 +868,7 @@ void LidarOdometry::onLidarImpl(const CObservation::Ptr & obs)
     initPose.mean = mrpt::poses::CPose3D::Identity();
     initPose.cov.setDiagonal(1e-12);
 
-    state_.navstate_fuse.fuse_pose(this_obs_tim, initPose, NAVSTATE_LIODOM_FRAME);
+    state_.navstate_fuse->fuse_pose(this_obs_tim, initPose, NAVSTATE_LIODOM_FRAME);
   } else {
     // Register point clouds using ICP:
     // ------------------------------------
@@ -1062,7 +1076,7 @@ void LidarOdometry::onLidarImpl(const CObservation::Ptr & obs)
 
       if (state_.step_counter_post_relocalization == 0) {
         // Do integrate info:
-        state_.navstate_fuse.fuse_pose(
+        state_.navstate_fuse->fuse_pose(
           this_obs_tim, out.found_pose_to_wrt_from, NAVSTATE_LIODOM_FRAME);
       } else {
         // Skip during post-relocalization:
@@ -1071,7 +1085,7 @@ void LidarOdometry::onLidarImpl(const CObservation::Ptr & obs)
 
     } else {
       // Bad ICP:
-      state_.navstate_fuse.reset();
+      state_.navstate_fuse->reset();
     }
 
     // Update trajectory too:
@@ -1380,7 +1394,8 @@ void LidarOdometry::onIMUImpl(const CObservation::Ptr & o)
   MRPT_LOG_DEBUG_STREAM(
     "onIMU called for timestamp=" << mrpt::system::dateTimeLocalToString(imu->timestamp));
 
-  state_.navstate_fuse.fuse_imu(*imu);
+  ASSERT_(state_.navstate_fuse);
+  state_.navstate_fuse->fuse_imu(*imu);
 }
 
 void LidarOdometry::onWheelOdometry(const CObservation::Ptr & o)
@@ -1415,7 +1430,7 @@ void LidarOdometry::onWheelOdometryImpl(const CObservation::Ptr & o)
 
   MRPT_LOG_DEBUG_STREAM("onWheelOdometry: odom=" << odo->odometry);
 
-  state_.navstate_fuse.fuse_odometry(*odo);
+  state_.navstate_fuse->fuse_odometry(*odo);
 }
 
 void LidarOdometry::onGPS(const CObservation::Ptr & o)

mola-cli-launchs/lidar_odometry_from_kitti.yaml

@@ -10,7 +10,7 @@ modules:
   # =====================
   - name: viz
     type: mola::MolaViz
-    #verbosity_level: DEBUG
+    verbosity_level: '${MOLA_VERBOSITY_MOLA_VIZ|INFO}'
     params: ~ # none
 
 # Offline or online sensory data sources =====================
@@ -42,9 +42,16 @@ modules:
 
   - name: lidar_odom
     type: mola::LidarOdometry
-    #verbosity_level: DEBUG
+    verbosity_level: '${MOLA_VERBOSITY_MOLA_LO|INFO}'
     raw_data_source: 'dataset_input'
     # This includes here a whole block "params: (...)"
     # with the LIDAR-Odometry pipeline configuration:
     params: '${MOLA_ODOMETRY_PIPELINE_YAML|../pipelines/lidar3d-default.yaml}'
 
+  - name: state_estimation
+    type: '${MOLA_STATE_ESTIMATOR|mola::state_estimation_simple::StateEstimationSimple}'
+    verbosity_level: '${MOLA_VERBOSITY_MOLA_STATE_ESTIMATOR|INFO}'
+
+    # This includes here a whole block "params: (...)":
+    params: '${MOLA_STATE_ESTIMATOR_YAML|../state-estimator-params/state-estimation-simple.yaml}'
+

mola-cli-launchs/lidar_odometry_from_kitti360.yaml

@@ -10,7 +10,7 @@ modules:
   # =====================
   - name: viz
     type: mola::MolaViz
-    #verbosity_level: DEBUG
+    verbosity_level: '${MOLA_VERBOSITY_MOLA_VIZ|INFO}'
     params: ~ # none
 
 # Offline or online sensory data sources =====================
@@ -42,9 +42,16 @@ modules:
 
   - name: lidar_odom
     type: mola::LidarOdometry
-    #verbosity_level: DEBUG
+    verbosity_level: '${MOLA_VERBOSITY_MOLA_LO|INFO}'
     raw_data_source: 'dataset_input'
     # This includes here a whole block "params: (...)"
     # with the LIDAR-Odometry pipeline configuration:
     params: '${MOLA_ODOMETRY_PIPELINE_YAML|../pipelines/lidar3d-default.yaml}'
 
+  - name: state_estimation
+    type: '${MOLA_STATE_ESTIMATOR|mola::state_estimation_simple::StateEstimationSimple}'
+    verbosity_level: '${MOLA_VERBOSITY_MOLA_STATE_ESTIMATOR|INFO}'
+
+    # This includes here a whole block "params: (...)":
+    params: '${MOLA_STATE_ESTIMATOR_YAML|../state-estimator-params/state-estimation-simple.yaml}'
+

mola-cli-launchs/lidar_odometry_from_kitti_output_ros2.yaml

@@ -10,7 +10,7 @@ modules:
   # =====================
   - name: viz
     type: mola::MolaViz
-    #verbosity_level: DEBUG
+    verbosity_level: '${MOLA_VERBOSITY_MOLA_VIZ|INFO}'
     params: ~ # none
 
 # Offline or online sensory data sources =====================
@@ -42,15 +42,23 @@ modules:
 
   - name: lidar_odom
     type: mola::LidarOdometry
-    #verbosity_level: DEBUG
+    verbosity_level: '${MOLA_VERBOSITY_MOLA_LO|INFO}'
     raw_data_source: 'dataset_input'
     # This includes here a whole block "params: (...)"
     # with the LIDAR-Odometry pipeline configuration:
     params: '${MOLA_ODOMETRY_PIPELINE_YAML|../pipelines/lidar3d-default.yaml}'
 
+  - name: state_estimation
+    type: '${MOLA_STATE_ESTIMATOR|mola::state_estimation_simple::StateEstimationSimple}'
+    verbosity_level: '${MOLA_VERBOSITY_MOLA_STATE_ESTIMATOR|INFO}'
+
+    # This includes here a whole block "params: (...)":
+    params: '${MOLA_STATE_ESTIMATOR_YAML|../state-estimator-params/state-estimation-simple.yaml}'
+
   # MOLA -> ROS 2 bridge  =====================
   - name: ros2_bridge
     type: mola::BridgeROS2
+    verbosity_level: '${MOLA_VERBOSITY_BRIDGE_ROS2|INFO}'
 
     execution_rate: 20 # Hz