fix(state_estimation_simple): correct twist and initial-guess corruption when wheel odometry is active

mola_state_estimation_simple/include/mola_state_estimation_simple/StateEstimationSimple.h

@@ -150,12 +150,34 @@ class StateEstimationSimple : public mola::NavStateFilter
         std::optional<mrpt::math::CMatrixDouble66>     last_twist_cov;
         bool                                           pose_already_updated_with_odom = false;
 
+        // Per-source bookkeeping used by fuse_pose() to compute velocity from
+        // consecutive poses of the SAME source (LiDAR ICP), independently of
+        // whether odometry has since modified last_pose. Without this, fuse_pose()
+        // would compute incrPose = ICP_result - (ICP_prev + odom_accumulated),
+        // i.e. the odometry residual, rather than the true robot velocity.
+        //
+        // Also used by fuse_odometry_3d_pose() for 3D odometry deltas.
+        struct SourceState
+        {
+            std::optional<mrpt::poses::CPose3DPDFGaussian> last_pose;
+            std::optional<mrpt::Clock::time_point>         last_obs_tim;
+        };
+        std::map<std::string, SourceState> per_source;
+
         // To be built from parameters strings when changed.
         RegexCache do_process_imu_labels_re;
         RegexCache do_process_odometry_labels_re;
         RegexCache do_process_gnss_labels_re;
     };
 
+    // Integrates a CObservationRobotPose that comes from an odometry source
+    // (e.g. wheel encoders forwarded as 3D pose). Unlike fuse_pose(), this
+    // applies an incremental delta to last_pose (keeping it in the LiDAR SLAM
+    // frame) and does NOT update last_pose_obs_tim, so it never interferes with
+    // the LiDAR ICP timestamp used for dt validation and pose extrapolation.
+    void fuse_odometry_3d_pose(
+        const mrpt::obs::CObservationRobotPose& obs, const std::string& odomName);
+
     State                        state_;
     mutable std::recursive_mutex state_mtx_;
 };

mola_state_estimation_simple/src/StateEstimationSimple.cpp

@@ -72,23 +72,131 @@ void StateEstimationSimple::fuse_odometry(
 {
     auto lck = std::scoped_lock(state_mtx_);
 
-    // this will work well only for simple datasets with one odometry:
+    // Advance last_pose by the incremental 2D odometry delta:
     if (state_.last_odom_obs && state_.last_pose)
     {
-        const auto poseIncr = odom.odometry - state_.last_odom_obs->odometry;
-
+        const auto poseIncr    = odom.odometry - state_.last_odom_obs->odometry;
         state_.last_pose->mean = state_.last_pose->mean + mrpt::poses::CPose3D(poseIncr);
-
-        // We can skip velocity-based model, but retain the twist:
-        // state_.last_twist: do not modify
         state_.pose_already_updated_with_odom = true;
     }
-    // copy:
     state_.last_odom_obs = odom;
 
+    // Use wheel velocities when available: they give a correct, uncontaminated
+    // twist for de-skewing and sigma computation, independently of whether
+    // LiDAR ICP has produced a new pose yet.
+    if (odom.hasVelocities)
+    {
+        if (!state_.last_twist)
+        {
+            state_.last_twist.emplace();
+        }
+        auto& tw = *state_.last_twist;
+        tw.vx    = odom.velocityLocal.vx;
+        tw.vy    = odom.velocityLocal.vy;
+        tw.vz    = 0;
+        tw.wz    = odom.velocityLocal.omega;
+        // wx, wy: left as-is (set by fuse_imu() when IMU is active, or zero
+        // from default construction above when it is not).
+        // Note: fuse_imu() still overrides wx/wy/wz whenever it runs.
+
+        const double varXYZ = mrpt::square(0.1);  // [m²/s²]
+        const double varRot = mrpt::square(0.05);  // [rad²/s²]
+        auto&        cov    = state_.last_twist_cov.emplace();
+        cov.setDiagonal({varXYZ, varXYZ, varXYZ, varRot, varRot, varRot});
+
+        MRPT_LOG_DEBUG_STREAM("fuse_odometry: twist from velocityLocal: " << tw.asString());
+    }
+
     MRPT_LOG_DEBUG_STREAM("fuse_odometry: odom=" << odom.asString());
 }
 
+void StateEstimationSimple::fuse_odometry_3d_pose(
+    const mrpt::obs::CObservationRobotPose& obs, const std::string& odomName)
+{
+    auto lck = std::scoped_lock(state_mtx_);
+
+    // Apply sensor-to-base correction if the sensor is not at the origin:
+    auto sensedPose = obs.pose;
+    if (obs.sensorPose != mrpt::poses::CPose3D())
+    {
+        sensedPose = sensedPose + mrpt::poses::CPose3DPDFGaussian(-obs.sensorPose);
+    }
+
+    auto& src = state_.per_source[odomName];
+
+    // Compute and apply the incremental delta to last_pose, keeping it in the
+    // LiDAR SLAM frame rather than replacing it with the absolute odom pose
+    // (which lives in a potentially offset odometry reference frame).
+    if (src.last_pose.has_value() && state_.last_pose.has_value())
+    {
+        const double dt =
+            src.last_obs_tim ? mrpt::system::timeDifference(*src.last_obs_tim, obs.timestamp) : 0.0;
+
+        if (dt < 0)
+        {
+            MRPT_LOG_THROTTLE_WARN_STREAM(
+                5.0, "fuse_odometry_3d_pose(): backwards timestamp for source '"
+                         << odomName << "', dt=" << dt << ". Resetting source.");
+            src.last_pose    = sensedPose;
+            src.last_obs_tim = obs.timestamp;
+            return;
+        }
+
+        const auto delta       = sensedPose.mean - src.last_pose->mean;
+        state_.last_pose->mean = state_.last_pose->mean + delta;
+        // pose_already_updated_with_odom is NOT set here because
+        // last_pose_obs_tim is updated to obs.timestamp below, so
+        // estimated_navstate() will compute the correct dt and extrapolate
+        // normally. (Contrast with fuse_odometry() which does NOT update
+        // last_pose_obs_tim and must suppress extrapolation via the flag.)
+
+        // Derive twist from the per-source consecutive 3D odom poses.
+        // This gives the correct wheel-odometry velocity independently of how
+        // last_pose has been set by LiDAR ICP.
+        if (dt > 0 && dt < params.max_time_to_use_velocity_model)
+        {
+            auto&        tw     = state_.last_twist.emplace();
+            const auto   logRot = mrpt::poses::Lie::SO<3>::log(delta.getRotationMatrix());
+            const double dt2    = dt * dt;
+
+            tw.vx = delta.x() / dt;
+            tw.vy = delta.y() / dt;
+            tw.vz = delta.z() / dt;
+            tw.wx = logRot[0] / dt;
+            tw.wy = logRot[1] / dt;
+            tw.wz = logRot[2] / dt;
+
+            auto& twistCov = state_.last_twist_cov.emplace();
+            twistCov.setDiagonal(
+                {mrpt::square(params.sigma_relative_pose_linear) / dt2,
+                 mrpt::square(params.sigma_relative_pose_linear) / dt2,
+                 mrpt::square(params.sigma_relative_pose_linear) / dt2,
+                 mrpt::square(params.sigma_relative_pose_angular) / dt2,
+                 mrpt::square(params.sigma_relative_pose_angular) / dt2,
+                 mrpt::square(params.sigma_relative_pose_angular) / dt2});
+
+            MRPT_LOG_DEBUG_STREAM(
+                "fuse_odometry_3d_pose('" << odomName << "'): twist=" << tw.asString());
+        }
+    }
+
+    src.last_pose    = sensedPose;
+    src.last_obs_tim = obs.timestamp;
+
+    // Bootstrap last_pose when no SLAM source has set it yet, so that
+    // estimated_navstate() can return valid results when CObservationRobotPose
+    // is the sole pose source (e.g., in tests or lidar-odom-only setups).
+    // When fuse_pose() is also active it owns last_pose_obs_tim and overwrites
+    // it using the per-source src.last_obs_tim guard, so this update is safe.
+    if (!state_.last_pose.has_value())
+    {
+        state_.last_pose = sensedPose;
+    }
+    state_.last_pose_obs_tim = obs.timestamp;
+
+    MRPT_LOG_DEBUG_STREAM("fuse_odometry_3d_pose('" << odomName << "'): pose=" << sensedPose.mean);
+}
+
 void StateEstimationSimple::fuse_imu(const mrpt::obs::CObservationIMU& imu)
 {
     auto lck = std::scoped_lock(state_mtx_);
@@ -158,30 +266,39 @@ void StateEstimationSimple::fuse_gnss(const mrpt::obs::CObservationGPS& gps)
 
 void StateEstimationSimple::fuse_pose(
     const mrpt::Clock::time_point& timestamp, const mrpt::poses::CPose3DPDFGaussian& pose,
-    [[maybe_unused]] const std::string& frame_id)
+    const std::string& frame_id)
 {
     auto lck = std::scoped_lock(state_mtx_);
 
-    mrpt::poses::CPose3D incrPose;
-
-    // numerical sanity: variances>=0 (==0 allowed for some components only)
-    for (int i = 0; i < 6; i++)
-    {
-        ASSERT_GE_(pose.cov(i, i), .0);
-    }
-    // and the sum of all strictly >0
+    // Numerical sanity: variances >= 0 (== 0 allowed for some components only)
+    for (int i = 0; i < 6; i++) ASSERT_GE_(pose.cov(i, i), .0);
     ASSERT_GT_(pose.cov.trace(), .0);
 
+    // fuse_pose() is the exclusive path for the primary localization source
+    // (LiDAR ICP). Wheel-odometry CObservationRobotPose observations are
+    // routed to fuse_odometry_3d_pose() in onNewObservation() instead, so
+    // they never arrive here and cannot corrupt last_pose_obs_tim.
+
+    // Per-source bookkeeping for this localization source.
+    // We use src.last_pose rather than last_pose for the incrPose calculation
+    // so that the derived twist reflects true ICP-to-ICP motion even when
+    // fuse_odometry() / fuse_odometry_3d_pose() have modified last_pose in
+    // between ICP scans.
+    auto& src = state_.per_source[frame_id];
+
     double dt = 0;
-    if (state_.last_pose_obs_tim)
+    if (src.last_obs_tim)
     {
-        dt = mrpt::system::timeDifference(*state_.last_pose_obs_tim, timestamp);
+        dt = mrpt::system::timeDifference(*src.last_obs_tim, timestamp);
     }
 
     if (dt < 0)
     {
         MRPT_LOG_THROTTLE_WARN_STREAM(
-            5.0, "Ignoring fuse_pose() call with dt=" << dt << " frame_id=" << frame_id);
+            5.0, "Ignoring fuse_pose() call with backwards timestamp: dt=" << dt << " frame_id="
+                                                                           << frame_id);
+        src.last_obs_tim = timestamp;
+        src.last_pose    = pose;
         return;
     }
 
@@ -191,28 +308,30 @@ void StateEstimationSimple::fuse_pose(
         MRPT_LOG_DEBUG_STREAM("fuse_pose(): twist before=" << state_.last_twist->asString());
     }
 
-    if (dt < params.max_time_to_use_velocity_model && state_.last_pose)
+    if (src.last_pose.has_value() && dt > 0 && dt < params.max_time_to_use_velocity_model)
     {
-        auto& tw = state_.last_twist.emplace();
-
-        incrPose = pose.mean - (state_.last_pose)->mean;
+        // Velocity from consecutive ICP poses, uncontaminated by odometry
+        // updates to the shared last_pose between scans:
+        auto&        tw       = state_.last_twist.emplace();
+        const auto   incrPose = pose.mean - src.last_pose->mean;
+        const auto   logRot   = mrpt::poses::Lie::SO<3>::log(incrPose.getRotationMatrix());
+        const double dt2      = dt * dt;
 
         tw.vx = incrPose.x() / dt;
         tw.vy = incrPose.y() / dt;
         tw.vz = incrPose.z() / dt;
-
-        const auto logRot = mrpt::poses::Lie::SO<3>::log(incrPose.getRotationMatrix());
-
         tw.wx = logRot[0] / dt;
         tw.wy = logRot[1] / dt;
         tw.wz = logRot[2] / dt;
 
-        // Rough guess of the covariance of the twist:
-        auto&        twistCov = state_.last_twist_cov.emplace();
-        const double dt2      = dt * dt;
-        const double varXYZ   = mrpt::square(params.sigma_relative_pose_linear) / dt2;  // [m²/s²]
-        const double varRot = mrpt::square(params.sigma_relative_pose_angular) / dt2;  // [rad²/s²]
-        twistCov.setDiagonal({varXYZ, varXYZ, varXYZ, varRot, varRot, varRot});
+        auto& twistCov = state_.last_twist_cov.emplace();
+        twistCov.setDiagonal(
+            {mrpt::square(params.sigma_relative_pose_linear) / dt2,
+             mrpt::square(params.sigma_relative_pose_linear) / dt2,
+             mrpt::square(params.sigma_relative_pose_linear) / dt2,
+             mrpt::square(params.sigma_relative_pose_angular) / dt2,
+             mrpt::square(params.sigma_relative_pose_angular) / dt2,
+             mrpt::square(params.sigma_relative_pose_angular) / dt2});
     }
     else
     {
@@ -232,7 +351,9 @@ void StateEstimationSimple::fuse_pose(
             << state_.last_twist_cov->asString());
     }
 
-    // save for next iter:
+    src.last_pose    = pose;
+    src.last_obs_tim = timestamp;
+
     state_.last_pose                      = pose;
     state_.last_pose_obs_tim              = timestamp;
     state_.pose_already_updated_with_odom = false;
@@ -402,16 +523,11 @@ void StateEstimationSimple::onNewObservation(const CObservation::ConstPtr& o)
                 o->sensorLabel, state_.do_process_odometry_labels_re.get_regex(
                                     params.do_process_odometry_labels_re)))
         {
-            auto sensedSensorPose = obsPose->pose;
-            if (obsPose->sensorPose != mrpt::poses::CPose3D())
-            {
-                sensedSensorPose =
-                    sensedSensorPose + mrpt::poses::CPose3DPDFGaussian(-obsPose->sensorPose);
-            }
-
-            // This simple estimator is not frame-aware; use sensorLabel as frame_id
-            // for logging consistency with the smoother, but fuse_pose() ignores it.
-            this->fuse_pose(obsPose->timestamp, sensedSensorPose, obsPose->sensorLabel);
+            // Route to the dedicated 3D-odometry path so it never touches
+            // last_pose_obs_tim (which belongs to the LiDAR ICP source) and
+            // applies the pose as an incremental delta in the SLAM frame rather
+            // than replacing last_pose with the absolute odom-frame pose.
+            this->fuse_odometry_3d_pose(*obsPose, o->sensorLabel);
         }
         else
         {

mola_state_estimation_simple/tests/test-state-estimation-simple.cpp

@@ -343,6 +343,85 @@ void test_robot_pose_observation()
     std::cout << "OK\n";
 }
 
+// --------------------------------------------------------------------------
+// Test 7: Mixed ICP + 3-D wheel-odometry source
+// --------------------------------------------------------------------------
+// LiDAR ICP supplies the ground-truth trajectory via fuse_pose() at 1 Hz.
+// Wheel odometry arrives as CObservationRobotPose at 2 Hz and advances the
+// state between ICP scans via fuse_odometry_3d_pose().
+//
+// Key invariants checked:
+//  (a) The twist derived from two consecutive ICP poses is the true ICP-to-ICP
+//      velocity and is NOT contaminated by odometry deltas applied to
+//      state_.last_pose between those scans.
+//  (b) estimated_navstate() extrapolates correctly from the odom-advanced pose
+//      while an ICP scan is still pending.
+//  (c) estimated_navstate() extrapolates correctly from the ICP pose once a
+//      new scan has been fused.
+void test_icp_and_3d_odometry_fusion()
+{
+    std::cout << "[Test 7] ICP + 3-D odometry fusion... ";
+
+    mola::state_estimation_simple::StateEstimationSimple estimator;
+    estimator.setMinLoggingLevel(mrpt::system::LVL_DEBUG);
+    estimator.initialize(get_default_config());
+
+    const auto   cov = mrpt::math::CMatrixDouble66::Identity();
+    const double v_x = 1.0;  // m/s — robot moves steadily in +X
+
+    // Helper: inject a CObservationRobotPose for the wheel-odometry source.
+    auto send_wheel_odom = [&](double t, double x)
+    {
+        auto obs         = mrpt::obs::CObservationRobotPose::Create();
+        obs->timestamp   = mrpt::Clock::fromDouble(t);
+        obs->sensorLabel = "wheel_odom";
+        obs->pose        = mrpt::poses::CPose3DPDFGaussian(mrpt::poses::CPose3D(x, 0, 0), cov);
+        estimator.onNewObservation(obs);
+    };
+
+    // --- t=0: both sources initialise at origin ---
+    estimator.fuse_pose(
+        mrpt::Clock::fromDouble(0.0),
+        mrpt::poses::CPose3DPDFGaussian(mrpt::poses::CPose3D::Identity(), cov), "map");
+    send_wheel_odom(0.0, 0.0);
+
+    // --- t=0.5: odom fires mid-scan, advancing state_.last_pose to x=0.5 ---
+    send_wheel_odom(0.5, 0.5);
+
+    // (b) Mid-scan extrapolation: odom twist (1 m/s) from (0.5,0,0) at t=0.5
+    //     queried at t=0.7 → expected x = 0.5 + 1.0*0.2 = 0.7
+    {
+        auto s = estimator.estimated_navstate(mrpt::Clock::fromDouble(0.7), "map");
+        ASSERT_(s.has_value());
+        ASSERT_NEAR_(s->pose.mean.x(), 0.7, 1e-3);
+    }
+
+    // --- t=1: LiDAR ICP delivers the true vehicle position (1,0,0) ---
+    estimator.fuse_pose(
+        mrpt::Clock::fromDouble(1.0),
+        mrpt::poses::CPose3DPDFGaussian(mrpt::poses::CPose3D(v_x, 0, 0), cov), "map");
+
+    // (a) Twist must equal the ICP-to-ICP velocity (1.0 m/s).
+    //     A regression would give 0.5 m/s because the broken code used the
+    //     odom-advanced state_.last_pose=(0.5,0,0) as the base instead of the
+    //     per-source ICP pose=(0,0,0).
+    {
+        auto tw = estimator.get_last_twist();
+        ASSERT_(tw.has_value());
+        ASSERT_NEAR_(tw->vx, v_x, 1e-3);
+    }
+
+    // (c) Post-ICP extrapolation: ICP pose (1,0,0) at t=1, query at t=1.5
+    //     → expected x = 1.0 + 1.0*0.5 = 1.5
+    {
+        auto s = estimator.estimated_navstate(mrpt::Clock::fromDouble(1.5), "map");
+        ASSERT_(s.has_value());
+        ASSERT_NEAR_(s->pose.mean.x(), 1.5, 1e-3);
+    }
+
+    std::cout << "OK\n";
+}
+
 }  // namespace
 
 int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv)
@@ -355,6 +434,7 @@ int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv)
         test_planar_motion();
         test_gnss_ignored();
         test_robot_pose_observation();
+        test_icp_and_3d_odometry_fusion();
 
         std::cout << "\nAll StateEstimationSimple tests passed!\n";
         return 0;