12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.
master
Holger Rapp 2017-04-26 14:14:24 +02:00 committed by GitHub
parent 245e356449
commit b4b83405ce
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -201,7 +201,8 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
} }
// Improve the velocity estimate. // 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_); const double delta_t = common::ToSeconds(time - last_scan_match_time_);
velocity_estimate_ += (pose_estimate_.translation().head<2>() - velocity_estimate_ += (pose_estimate_.translation().head<2>() -
model_prediction.translation().head<2>()) / model_prediction.translation().head<2>()) /