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

Fix the dead loop. (#1586) 

Fix the bug in RotateHistogram which may cause dead loop by numerical error.
master
jie 2020-05-28 10:28:15 -07:00 committed by GitHub
parent bcd5486025
commit ad216d49c9
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 4 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc
View File

@ -140,9 +140,13 @@ RotationalScanMatcher::RotationalScanMatcher(const Eigen::VectorXf* histogram)
// rotations of a fractional bucket which is handled by linearly interpolating. // rotations of a fractional bucket which is handled by linearly interpolating.
Eigen::VectorXf RotationalScanMatcher::RotateHistogram( Eigen::VectorXf RotationalScanMatcher::RotateHistogram(
const Eigen::VectorXf& histogram, const float angle) { const Eigen::VectorXf& histogram, const float angle) {
if (histogram.size() == 0) {
return histogram;
}
const float rotate_by_buckets = -angle * histogram.size() / M_PI; const float rotate_by_buckets = -angle * histogram.size() / M_PI;
int full_buckets = common::RoundToInt(rotate_by_buckets - 0.5f); int full_buckets = common::RoundToInt(rotate_by_buckets - 0.5f);
const float fraction = rotate_by_buckets - full_buckets; const float fraction = rotate_by_buckets - full_buckets;
CHECK_GT(histogram.size(), 0);
while (full_buckets < 0) { while (full_buckets < 0) {
full_buckets += histogram.size(); full_buckets += histogram.size();
} }