diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 9100086125..4f6413012c 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -201,7 +201,8 @@ LocalTrajectoryBuilder::AddHorizontalRangeData( } // Improve the velocity estimate. - if (last_scan_match_time_ > common::Time::min()) { + if (last_scan_match_time_ > common::Time::min() && + time > last_scan_match_time_) { const double delta_t = common::ToSeconds(time - last_scan_match_time_); velocity_estimate_ += (pose_estimate_.translation().head<2>() - model_prediction.translation().head<2>()) /