Fix MapById::lower_bound function and implement test to reproduce the bug (#1616)

This fixes the bug that has been faced during localization with an initial pose and a trimmed map (OverlappingSubmapsTrimmer2D has been used). A small test is added that reproduces the problem (an infinite loop) where a trimmed trajectory (MapById instance) is used.
master
Mariia Gladkova 2020-06-12 12:17:54 +02:00 committed by GitHub
parent db85e0894d
commit d973a084f7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 57 additions and 1 deletions

View File

@ -378,14 +378,23 @@ class MapById {
const std::map<int, DataType>& trajectory = const std::map<int, DataType>& trajectory =
trajectories_.at(trajectory_id).data_; trajectories_.at(trajectory_id).data_;
if (internal::GetTime(std::prev(trajectory.end())->second) < time) { if (internal::GetTime(std::prev(trajectory.end())->second) < time) {
return EndOfTrajectory(trajectory_id); return EndOfTrajectory(trajectory_id);
} }
auto left = trajectory.begin(); auto left = trajectory.begin();
auto right = std::prev(trajectory.end()); auto right = std::prev(trajectory.end());
while (left != right) { while (left != right) {
// This is never 'right' which is important to guarantee progress.
const int middle = left->first + (right->first - left->first) / 2; const int middle = left->first + (right->first - left->first) / 2;
const auto lower_bound_middle = trajectory.lower_bound(middle); // This could be 'right' in the presence of gaps, so we need to use the
// previous element in this case.
auto lower_bound_middle = trajectory.lower_bound(middle);
if (lower_bound_middle->first > middle) {
CHECK(lower_bound_middle != left);
lower_bound_middle = std::prev(lower_bound_middle);
}
if (internal::GetTime(lower_bound_middle->second) < time) { if (internal::GetTime(lower_bound_middle->second) < time) {
left = std::next(lower_bound_middle); left = std::next(lower_bound_middle);
} else { } else {

View File

@ -254,6 +254,53 @@ TEST(IdTest, LowerBoundFuzz) {
} }
} }
TEST(IdTest, LowerBoundTrimmedTrajectory) {
constexpr int kTrajectoryId = 1;
std::mt19937 rng;
std::uniform_int_distribution<int> dt_dist(1, 20);
const int N = 500;
int t = 0;
MapById<SubmapId, Data> map_by_id;
for (int j = 0; j < N; ++j) {
t = t + dt_dist(rng);
map_by_id.Append(kTrajectoryId, Data(t));
}
// Choose random length of a trim segment.
std::uniform_int_distribution<int> dt_trim_segment_length(
1, static_cast<int>(N / 2));
size_t trim_segment_length = dt_trim_segment_length(rng);
// Choose random start for a trim_segment.
std::uniform_int_distribution<int> dt_trim_segment_start(
2, N - trim_segment_length - 1);
size_t trim_segment_start_index = dt_trim_segment_start(rng);
auto trim_segment_start = map_by_id.begin();
std::advance(trim_segment_start, trim_segment_start_index);
auto trim_segment_end = map_by_id.begin();
std::advance(trim_segment_end,
trim_segment_start_index + trim_segment_length);
for (auto it = trim_segment_start; it != trim_segment_end;) {
const auto this_it = it;
++it;
map_by_id.Trim(this_it->id);
}
auto it = map_by_id.lower_bound(kTrajectoryId, CreateTime(0));
auto ground_truth =
std::lower_bound(map_by_id.BeginOfTrajectory(kTrajectoryId),
map_by_id.EndOfTrajectory(kTrajectoryId), CreateTime(0),
[](MapById<SubmapId, Data>::IdDataReference a,
const common::Time& t) { return a.data.time() < t; });
EXPECT_EQ(ground_truth, it);
}
struct DataStruct { struct DataStruct {
const common::Time time; const common::Time time;
}; };