Skip to content

Commit

Permalink
Fix divison by zero.
Browse files Browse the repository at this point in the history
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 cartographer-project#233.
  • Loading branch information
SirVer committed Apr 26, 2017
1 parent 245e356 commit 75aa416
Showing 1 changed file with 2 additions and 1 deletion.
3 changes: 2 additions & 1 deletion cartographer/mapping_2d/local_trajectory_builder.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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>()) /
Expand Down

0 comments on commit 75aa416

Please sign in to comment.