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

Add FindChecked to MapById. (#586) 

* Add FindChecked to MapById.
master
Christoph Schütte 2017-10-16 14:15:53 +02:00 committed by GitHub
parent d91afa4496
commit 6b447c4577
2 changed files with 38 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
cartographer/mapping/id.h
View File

@ -158,6 +158,16 @@ class MapById {
}
}
explicit ConstIterator(const MapById& map_by_id, const IdType& id)
: current_trajectory_(
map_by_id.trajectories_.find(id.trajectory_id)),
end_trajectory_(map_by_id.trajectories_.end()) {
CHECK(current_trajectory_ != end_trajectory_);
current_data_ =
current_trajectory_->second.data_.find(MapById::GetIndex(id));
CHECK(current_data_ != current_trajectory_->second.data_.end());
}
IdDataReference operator*() const {
CHECK(current_trajectory_ != end_trajectory_);
return IdDataReference{
@ -262,6 +272,11 @@ class MapById {
return IdType{trajectory_id, index};
}
// Returns an iterator to the element at 'id' which must exist.
ConstIterator FindChecked(const IdType& id) {
return ConstIterator(*this, id);
}
// Inserts data (which must not exist already) into a trajectory.
void Insert(const IdType& id, const DataType& data) {
auto& trajectory = trajectories_[id.trajectory_id];

29
cartographer/mapping/id_test.cc
View File

@ -26,8 +26,9 @@ namespace cartographer {
namespace mapping {
namespace {
static MapById<NodeId, int> CreateTestMapById() {
MapById<NodeId, int> map_by_id;
template <typename IdType>
static MapById<IdType, int> CreateTestMapById() {
MapById<IdType, int> map_by_id;
map_by_id.Append(7, 2);
map_by_id.Append(42, 3);
map_by_id.Append(0, 0);
@ -45,7 +46,7 @@ TEST(IdTest, EmptyMapById) {
}
TEST(IdTest, MapByIdIterator) {
MapById<NodeId, int> map_by_id = CreateTestMapById();
MapById<NodeId, int> map_by_id = CreateTestMapById<NodeId>();
EXPECT_EQ(2, map_by_id.BeginOfTrajectory(7)->data);
EXPECT_TRUE(std::next(map_by_id.BeginOfTrajectory(7)) ==
map_by_id.EndOfTrajectory(7));
@ -65,7 +66,7 @@ TEST(IdTest, MapByIdIterator) {
}
TEST(IdTest, MapByIdTrajectoryRange) {
MapById<NodeId, int> map_by_id = CreateTestMapById();
MapById<NodeId, int> map_by_id = CreateTestMapById<NodeId>();
std::deque<std::pair<NodeId, int>> expected_data = {
{NodeId{0, 0}, 0},
{NodeId{0, 1}, 1},
@ -80,7 +81,7 @@ TEST(IdTest, MapByIdTrajectoryRange) {
}
TEST(IdTest, MapByIdTrajectoryIdRange) {
MapById<NodeId, int> map_by_id = CreateTestMapById();
MapById<NodeId, int> map_by_id = CreateTestMapById<NodeId>();
std::deque<int> expected_data = {0, 7, 42};
for (const int trajectory_id : map_by_id.trajectory_ids()) {
EXPECT_EQ(expected_data.front(), trajectory_id);
@ -91,7 +92,7 @@ TEST(IdTest, MapByIdTrajectoryIdRange) {
}
TEST(IdTest, MapByIdIterateByTrajectories) {
MapById<NodeId, int> map_by_id = CreateTestMapById();
MapById<NodeId, int> map_by_id = CreateTestMapById<NodeId>();
std::deque<std::pair<NodeId, int>> expected_id_data = {
{NodeId{0, 0}, 0},
{NodeId{0, 1}, 1},
@ -126,6 +127,22 @@ TEST(IdTest, InsertIntoMapById) {
EXPECT_EQ(2, map_by_id.SizeOfTrajectoryOrZero(42));
}
TEST(IdTest, FindCheckedNodeId) {
MapById<NodeId, int> map_by_id;
map_by_id.Append(42, 42);
map_by_id.Append(42, 43);
map_by_id.Append(42, 44);
CHECK_EQ(map_by_id.FindChecked(NodeId{42, 1})->data, 43);
}
TEST(IdTest, FindCheckedSubmapId) {
MapById<SubmapId, int> map_by_id;
map_by_id.Append(42, 42);
map_by_id.Append(42, 43);
map_by_id.Append(42, 44);
CHECK_EQ(map_by_id.FindChecked(SubmapId{42, 1})->data, 43);
}
} // namespace
} // namespace mapping
} // namespace cartographer