diff --git a/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h b/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h index bd9e4e04..f15a9c30 100644 --- 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 last_pose_obs_tim; std::optional last_pose; std::optional last_twist; + std::optional 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 index aec6f293..a6846b9a 100644 --- 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)