From ad216d49c966a046073765207491ad1e479c4413 Mon Sep 17 00:00:00 2001 From: jie Date: Thu, 28 May 2020 10:28:15 -0700 Subject: [PATCH] Fix the dead loop. (#1586) Fix the bug in RotateHistogram which may cause dead loop by numerical error. --- .../internal/3d/scan_matching/rotational_scan_matcher.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc index 712cf89..0e4b5e0 100644 --- a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc +++ b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc @@ -140,9 +140,13 @@ RotationalScanMatcher::RotationalScanMatcher(const Eigen::VectorXf* histogram) // rotations of a fractional bucket which is handled by linearly interpolating. Eigen::VectorXf RotationalScanMatcher::RotateHistogram( const Eigen::VectorXf& histogram, const float angle) { + if (histogram.size() == 0) { + return histogram; + } const float rotate_by_buckets = -angle * histogram.size() / M_PI; int full_buckets = common::RoundToInt(rotate_by_buckets - 0.5f); const float fraction = rotate_by_buckets - full_buckets; + CHECK_GT(histogram.size(), 0); while (full_buckets < 0) { full_buckets += histogram.size(); }