Rewrite NavStateFuse as a proper sliding-window factor graph optimiza…

…tion

mola_navstate_fuse/CMakeLists.txt

@@ -18,8 +18,10 @@ find_package(mola_common REQUIRED)
 
 # find CMake dependencies:
 find_package(mrpt-obs)
+find_package(GTSAM REQUIRED)
 
 # Find MOLA packages:
+find_package(mola_kernel REQUIRED)
 find_package(mola_imu_preintegration REQUIRED)
 
 # -----------------------
@@ -32,13 +34,16 @@ mola_add_library(
 	SOURCES ${LIB_SRCS} ${LIB_PUBLIC_HDRS}
 	PUBLIC_LINK_LIBRARIES
 		mrpt::obs
+		mola::mola_kernel
 		mola::mola_imu_preintegration
-	#PRIVATE_LINK_LIBRARIES
-	#	mrpt::obs
+	PRIVATE_LINK_LIBRARIES
+		gtsam
 	CMAKE_DEPENDENCIES
 		mola_common
 		mola_imu_preintegration
+		mola_kernel
 )
+target_include_directories(${PROJECT_NAME} PRIVATE ".")
 
 # -----------------------
 # define tests:

mola_navstate_fuse/include/mola_navstate_fuse/NavState.h

@@ -30,6 +30,10 @@
 
 namespace mola
 {
+/** The state returned by NavStateFuse
+ *
+ * \ingroup mola_navstate_fuse_grp
+ */
 struct NavState
 {
     NavState()  = default;

mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h

@@ -25,20 +25,50 @@
  */
 #pragma once
 
-#include <mola_imu_preintegration/RotationIntegrator.h>
+// this package:
 #include <mola_navstate_fuse/NavState.h>
 #include <mola_navstate_fuse/NavStateFuseParams.h>
+
+// MOLA:
+#include <mola_imu_preintegration/RotationIntegrator.h>
+#include <mola_kernel/factors/FactorConstVelKinematics.h>
+
+// MRPT:
+#include <mrpt/containers/bimap.h>
 #include <mrpt/containers/yaml.h>
-#include <mrpt/core/optional_ref.h>
+#include <mrpt/core/pimpl.h>
 #include <mrpt/obs/CObservationIMU.h>
 #include <mrpt/obs/CObservationOdometry.h>
 #include <mrpt/poses/CPose3DPDFGaussian.h>
+#include <mrpt/system/COutputLogger.h>
 
+// std:
+#include <mutex>
 #include <optional>
 
 namespace mola
 {
-/** Fuse of odometry, IMU, and SE(3) pose/twist estimations.
+/** Sliding window Factor-graph data fusion for odometry, IMU, GNNS, and SE(3)
+ * pose/twist estimations.
+ *
+ * Frame conventions:
+ * - There is a frame of reference for each source of odometry, e.g.
+ *   there may be one for LiDAR-odometry, another for visual-odometry, or
+ *   wheels-based odometry, etc. Each such frame is referenced with a "frame
+ *   name" (an arbitrary string).
+ * - Internally, the first frame of reference will be used as "global"
+ *   coordinates, despite it may be actually either a `map` or `odom` frame, in
+ *   the [ROS REP 105](https://www.ros.org/reps/rep-0105.html) sense.
+ * - IMU readings are, by definition, given in the robot body frame, although
+ *   they can have a relative transformation between the vehicle and sensor.
+ *
+ * Main API methods and frame conventions:
+ * - `estimated_navstate()`: Output estimations can be requested in any of the
+ *    existing frames of reference.
+ * - `fuse_pose()`: Can be used to integrate information from any "odometry" or
+ *   "localization" input, as mentioned above.
+ * - `fuse_gnns()`: TO-DO.
+ * - `fuse_imu()`: TO-DO.
  *
  * Usage:
  * - (1) Call initialize() or set the required parameters directly in params_.
@@ -51,13 +81,18 @@ namespace mola
  *
  * Old observations are automatically removed.
  *
- * \sa IMUIntegrator
- * \ingroup mola_imu_preintegration_grp
+ * A constant SE(3) velocity model is internally used, without any
+ * particular assumptions on the vehicle kinematics.
+ *
+ * For more theoretical descriptions, see the papers cited in
+ * https://docs.mola-slam.org/latest/
+ *
+ * \ingroup mola_navstate_fuse_grp
  */
-class NavStateFuse
+class NavStateFuse : public mrpt::system::COutputLogger
 {
    public:
-    NavStateFuse()  = default;
+    NavStateFuse();
     ~NavStateFuse() = default;
 
     /** \name Main API
@@ -76,17 +111,25 @@ class NavStateFuse
      */
     void reset();
 
-    /** Integrates new odometry observations into the estimator */
-    void fuse_odometry(const mrpt::obs::CObservationOdometry& odom);
+    /** Integrates new SE(3) pose estimation of the vehicle wrt frame_id
+     */
+    void fuse_pose(
+        const mrpt::Clock::time_point&         timestamp,
+        const mrpt::poses::CPose3DPDFGaussian& pose,
+        const std::string&                     frame_id);
+
+    /** Integrates new wheels-based odometry observations into the estimator.
+     *  This is a convenience method that internally ends up calling
+     *  fuse_pose(), but computing the uncertainty of odometry increments
+     *  according to a given motion model.
+     */
+    void fuse_odometry(
+        const mrpt::obs::CObservationOdometry& odom,
+        const std::string&                     odomName = "odom_wheels");
 
     /** Integrates new IMU observations into the estimator */
     void fuse_imu(const mrpt::obs::CObservationIMU& imu);
 
-    /** Integrates new SE(3) pose estimation into the estimator */
-    void fuse_pose(
-        const mrpt::Clock::time_point&         timestamp,
-        const mrpt::poses::CPose3DPDFGaussian& pose);
-
     /** Integrates new twist estimation (in the odom frame) */
     void fuse_twist(
         const mrpt::Clock::time_point& timestamp,
@@ -98,31 +141,83 @@ class NavStateFuse
      * validity time window (e.g. too far in the future to be trustful).
      */
     std::optional<NavState> estimated_navstate(
-        const mrpt::Clock::time_point& timestamp) const;
+        const mrpt::Clock::time_point& timestamp);
 
+#if 0
     std::optional<mrpt::math::TTwist3D> get_last_twist() const
     {
         return state_.last_twist;
     }
 
     void force_last_twist(const mrpt::math::TTwist3D& twist);
+#endif
 
     /** @} */
 
    private:
+    // everything related to gtsam is hidden in the public API via pimpl
+    struct GtsamImpl;
+
+    using frameid_t = uint8_t;
+
+    // an observation from fuse_pose()
+    struct PoseData
+    {
+        PoseData() = default;
+
+        mrpt::poses::CPose3DPDFGaussian pose;
+        frameid_t                       frameId;
+    };
+
+    // Dummy type representing the query point.
+    struct QueryPointData
+    {
+        QueryPointData() = default;
+    };
+
+    struct PointData
+    {
+        PointData(const PoseData& p) : pose(p) {}
+        PointData(const QueryPointData& p) : query(p) {}
+
+        std::optional<PoseData>       pose;
+        std::optional<QueryPointData> query;
+
+       private:
+        PointData() = default;
+    };
+
     struct State
     {
-        State()  = default;
-        ~State() = default;
+        State();
+        ~State();
+
+        mrpt::pimpl<GtsamImpl> impl;
+
+        /// A bimap of known "frame_id" <=> "numeric IDs":
+        mrpt::containers::bimap<std::string, frameid_t> known_frames;
+
+        /// Returns the existing ID, or creates a new ID, for a frame:
+        frameid_t frame_id(const std::string& frame_name);
+
+        /// The sliding window of observation data:
+        std::multimap<mrpt::Clock::time_point, PointData> data;
 
+#if 0
         std::optional<mrpt::Clock::time_point>         last_pose_obs_tim;
+        std::optional<mrpt::obs::CObservationOdometry> last_odom_obs;
         std::optional<mrpt::poses::CPose3DPDFGaussian> last_pose;
         std::optional<mrpt::math::TTwist3D>            last_twist;
-        std::optional<mrpt::obs::CObservationOdometry> last_odom_obs;
+#endif
     };
-    // const State& current_state() const { return state_; }
 
-    State state_;
+    State                state_;
+    std::recursive_mutex state_mtx_;  /// to access state_
+
+    void build_and_optimize_fg(const mrpt::Clock::time_point queryTimestamp);
+
+    /// Implementation of Eqs (1),(4) in the MOLA RSS2019 paper.
+    void addFactor(const mola::FactorConstVelKinematics& f);
 };
 
 }  // namespace mola

mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuseParams.h

@@ -33,7 +33,7 @@ namespace mola
 {
 /** Parameters needed by NavStateFuse.
  *
- * \ingroup mola_imu_preintegration_grp
+ * \ingroup mola_navstate_fuse__grp
  */
 class NavStateFuseParams
 {
@@ -45,11 +45,25 @@ class NavStateFuseParams
     void loadFrom(const mrpt::containers::yaml& cfg);
 
     /** Valid estimations will be extrapolated only up to this time since the
-     * last incorporated observation. */
+     * last incorporated observation. If a request is done farther away, an
+     * empty estimation will be returned.
+     */
     double max_time_to_use_velocity_model = 2.0;  // [s]
 
+    /// Time to keep past observations in the filter
+    double sliding_window_length = 5.0;  // [s]
+
+    /// If the time between two keyframes is larger than this, a warning will be
+    /// emitted; but the algorithm will keep trying its best.
+    double time_between_frames_to_warning = 3.0;  // [s]
+
     double sigma_random_walk_acceleration_linear  = 1.0;  // [m/s²]
     double sigma_random_walk_acceleration_angular = 1.0;  // [rad/s²]
+    double sigma_integrator_position              = 0.10;  // [m]
+    double sigma_integrator_orientation           = 0.10;  // [rad]
+
+    /** Const. velocity model: sigma of lin and angular velocity */
+    double const_vel_model_std_linvel{1.0}, const_vel_model_std_angvel{1.0};
 };
 
 }  // namespace mola

mola_navstate_fuse/package.xml

@@ -16,8 +16,10 @@
   <license file="LICENSE">GPLv3</license>
 
   <depend>mola_common</depend>
+  <depend>mola_kernel</depend>
   <depend>mola_imu_preintegration</depend>
   <depend>mrpt2</depend>
+  <depend>gtsam</depend>
 
   <doc_depend>doxygen</doc_depend>