use odometry

MOLAorg · Feb 17, 2024 · 6f2db19 · 6f2db19
1 parent 0fd770e
commit 6f2db19
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 2 deletions.
diff --git a/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h b/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h
@@ -118,6 +118,7 @@ class NavStateFuse
         std::optional<mrpt::Clock::time_point>         last_pose_obs_tim;
         std::optional<mrpt::poses::CPose3DPDFGaussian> last_pose;
         std::optional<mrpt::math::TTwist3D>            last_twist;
+        std::optional<mrpt::obs::CObservationOdometry> last_odom_obs;
     };
     // const State& current_state() const { return state_; }
 

diff --git a/mola_navstate_fuse/src/NavStateFuse.cpp b/mola_navstate_fuse/src/NavStateFuse.cpp
@@ -45,8 +45,21 @@ void NavStateFuse::reset()
 
 void NavStateFuse::fuse_odometry(const mrpt::obs::CObservationOdometry& odom)
 {
-    // TODO(jlbc)
-    (void)odom;
+    // TODO(jlbc): proper time-based data fusion.
+
+    // temporarily, this will work well only for simple datasets:
+    if (state_.last_odom_obs && state_.last_pose)
+    {
+        const auto poseIncr = odom.odometry - state_.last_odom_obs->odometry;
+
+        state_.last_pose->mean =
+            state_.last_pose->mean + mrpt::poses::CPose3D(poseIncr);
+
+        // and discard velocity-based model:
+        state_.last_twist = mrpt::math::TTwist3D(0, 0, 0, 0, 0, 0);
+    }
+    // copy:
+    state_.last_odom_obs = odom;
 }
 
 void NavStateFuse::fuse_imu(const mrpt::obs::CObservationIMU& imu)