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

Extend MapById::lower_bound() to support structs with 'time' field. (#871) 

[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)
master
Alexander Belyaev 2018-02-01 11:58:13 +01:00 committed by GitHub
parent eb3e63bad6
commit e735203a05
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 53 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
cartographer/mapping/id.h
View File

@ -35,6 +35,22 @@
namespace cartographer {
namespace mapping {
namespace internal {
template <class T>
auto GetTimeImpl(const T& t, int) -> decltype(t.time()) {
return t.time();
}
template <class T>
auto GetTimeImpl(const T& t, unsigned) -> decltype(t.time) {
return t.time;
}
template <class T>
common::Time GetTime(const T& t) {
return GetTimeImpl(t, 0);
}
} // namespace internal
// Uniquely identifies a trajectory node using a combination of a unique
// trajectory ID and a zero-based index of the node inside that trajectory.
@ -347,7 +363,7 @@ class MapById {
const std::map<int, DataType>& trajectory =
trajectories_.at(trajectory_id).data_;
if (std::prev(trajectory.end())->second.time() < time) {
if (internal::GetTime(std::prev(trajectory.end())->second) < time) {
return EndOfTrajectory(trajectory_id);
}
auto left = trajectory.begin();
@ -355,7 +371,7 @@ class MapById {
while (left != right) {
const int middle = left->first + (right->first - left->first) / 2;
const auto lower_bound_middle = trajectory.lower_bound(middle);
if (lower_bound_middle->second.time() < time) {
if (internal::GetTime(lower_bound_middle->second) < time) {
left = std::next(lower_bound_middle);
} else {
right = lower_bound_middle;

35
cartographer/mapping/id_test.cc
View File

@ -221,6 +221,41 @@ TEST(IdTest, LowerBoundFuzz) {
}
}
struct DataStruct {
const common::Time time;
};
TEST(IdTest, LowerBoundFuzzWithStruct) {
constexpr int kMaxTimeIncrement = 20;
constexpr int kMaxNumNodes = 20;
constexpr int kNumTests = 100;
constexpr int kTrajectoryId = 1;
std::mt19937 rng;
std::uniform_int_distribution<int> dt_dist(1, kMaxTimeIncrement);
std::uniform_int_distribution<int> N_dist(1, kMaxNumNodes);
for (int i = 0; i < kNumTests; ++i) {
const int N = N_dist(rng);
int t = 0;
MapById<SubmapId, DataStruct> map_by_id;
for (int j = 0; j < N; ++j) {
t = t + dt_dist(rng);
map_by_id.Append(kTrajectoryId, DataStruct{CreateTime(t)});
}
std::uniform_int_distribution<int> t0_dist(1, N * kMaxTimeIncrement + 1);
int t0 = t0_dist(rng);
auto it = map_by_id.lower_bound(kTrajectoryId, CreateTime(t0));
auto ground_truth = std::lower_bound(
map_by_id.BeginOfTrajectory(kTrajectoryId),
map_by_id.EndOfTrajectory(kTrajectoryId), CreateTime(t0),
[](MapById<SubmapId, DataStruct>::IdDataReference a,
const common::Time& t) { return a.data.time < t; });
CHECK(ground_truth == it);
}
}
} // namespace
} // namespace mapping
} // namespace cartographer