parent
d91afa4496
commit
6b447c4577
|
@ -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 {
|
IdDataReference operator*() const {
|
||||||
CHECK(current_trajectory_ != end_trajectory_);
|
CHECK(current_trajectory_ != end_trajectory_);
|
||||||
return IdDataReference{
|
return IdDataReference{
|
||||||
|
@ -262,6 +272,11 @@ class MapById {
|
||||||
return IdType{trajectory_id, index};
|
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.
|
// Inserts data (which must not exist already) into a trajectory.
|
||||||
void Insert(const IdType& id, const DataType& data) {
|
void Insert(const IdType& id, const DataType& data) {
|
||||||
auto& trajectory = trajectories_[id.trajectory_id];
|
auto& trajectory = trajectories_[id.trajectory_id];
|
||||||
|
|
|
@ -26,8 +26,9 @@ namespace cartographer {
|
||||||
namespace mapping {
|
namespace mapping {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
static MapById<NodeId, int> CreateTestMapById() {
|
template <typename IdType>
|
||||||
MapById<NodeId, int> map_by_id;
|
static MapById<IdType, int> CreateTestMapById() {
|
||||||
|
MapById<IdType, int> map_by_id;
|
||||||
map_by_id.Append(7, 2);
|
map_by_id.Append(7, 2);
|
||||||
map_by_id.Append(42, 3);
|
map_by_id.Append(42, 3);
|
||||||
map_by_id.Append(0, 0);
|
map_by_id.Append(0, 0);
|
||||||
|
@ -45,7 +46,7 @@ TEST(IdTest, EmptyMapById) {
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(IdTest, MapByIdIterator) {
|
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_EQ(2, map_by_id.BeginOfTrajectory(7)->data);
|
||||||
EXPECT_TRUE(std::next(map_by_id.BeginOfTrajectory(7)) ==
|
EXPECT_TRUE(std::next(map_by_id.BeginOfTrajectory(7)) ==
|
||||||
map_by_id.EndOfTrajectory(7));
|
map_by_id.EndOfTrajectory(7));
|
||||||
|
@ -65,7 +66,7 @@ TEST(IdTest, MapByIdIterator) {
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(IdTest, MapByIdTrajectoryRange) {
|
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 = {
|
std::deque<std::pair<NodeId, int>> expected_data = {
|
||||||
{NodeId{0, 0}, 0},
|
{NodeId{0, 0}, 0},
|
||||||
{NodeId{0, 1}, 1},
|
{NodeId{0, 1}, 1},
|
||||||
|
@ -80,7 +81,7 @@ TEST(IdTest, MapByIdTrajectoryRange) {
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(IdTest, MapByIdTrajectoryIdRange) {
|
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};
|
std::deque<int> expected_data = {0, 7, 42};
|
||||||
for (const int trajectory_id : map_by_id.trajectory_ids()) {
|
for (const int trajectory_id : map_by_id.trajectory_ids()) {
|
||||||
EXPECT_EQ(expected_data.front(), trajectory_id);
|
EXPECT_EQ(expected_data.front(), trajectory_id);
|
||||||
|
@ -91,7 +92,7 @@ TEST(IdTest, MapByIdTrajectoryIdRange) {
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(IdTest, MapByIdIterateByTrajectories) {
|
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 = {
|
std::deque<std::pair<NodeId, int>> expected_id_data = {
|
||||||
{NodeId{0, 0}, 0},
|
{NodeId{0, 0}, 0},
|
||||||
{NodeId{0, 1}, 1},
|
{NodeId{0, 1}, 1},
|
||||||
|
@ -126,6 +127,22 @@ TEST(IdTest, InsertIntoMapById) {
|
||||||
EXPECT_EQ(2, map_by_id.SizeOfTrajectoryOrZero(42));
|
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
|
||||||
} // namespace mapping
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
Loading…
Reference in New Issue