From b4b83405ce4009ea0c1ac22c7ab9edeeb9d48a42 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Wed, 26 Apr 2017 14:14:24 +0200 Subject: [PATCH] Fix divison by zero. (#240) If mapping_2d::LocalTrajectoryBuilder::AddHorizontalRangeData is called twice in a row with the same `time`, the `velocity_estimate_` becomes `inf` which led to `inf`s in the optimization problem, which led to failures inside Ceres. Fixes #233. --- cartographer/mapping_2d/local_trajectory_builder.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 9100086..4f64130 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>()) /