16 #include <gtest/gtest.h> 27 Eigen::Array2i submapTopLeftIndex(3, 1);
28 Eigen::Array2i submapBufferSize(3, 2);
30 Eigen::Array2i submapIndex;
33 types.emplace_back(
"type");
35 map.
setGeometry(Eigen::Array2d(8.1, 5.1), 1.0, Eigen::Vector2d(0.0, 0.0));
37 SubmapIterator iterator(map, submapTopLeftIndex, submapBufferSize);
40 EXPECT_EQ(submapTopLeftIndex(0), (*iterator)(0));
41 EXPECT_EQ(submapTopLeftIndex(1), (*iterator)(1));
47 EXPECT_EQ(3, (*iterator)(0));
48 EXPECT_EQ(2, (*iterator)(1));
54 EXPECT_EQ(4, (*iterator)(0));
55 EXPECT_EQ(1, (*iterator)(1));
61 EXPECT_EQ(4, (*iterator)(0));
62 EXPECT_EQ(2, (*iterator)(1));
68 EXPECT_EQ(5, (*iterator)(0));
69 EXPECT_EQ(1, (*iterator)(1));
75 EXPECT_EQ(5, (*iterator)(0));
76 EXPECT_EQ(2, (*iterator)(1));
82 EXPECT_EQ(5, (*iterator)(0));
83 EXPECT_EQ(2, (*iterator)(1));
89 Eigen::Array2i submapTopLeftIndex(6, 3);
90 Eigen::Array2i submapBufferSize(2, 4);
92 Eigen::Array2i submapIndex;
95 types.emplace_back(
"type");
100 SubmapIterator iterator(map, submapTopLeftIndex, submapBufferSize);
103 EXPECT_EQ(submapTopLeftIndex(0), (*iterator)(0));
104 EXPECT_EQ(submapTopLeftIndex(1), (*iterator)(1));
110 EXPECT_EQ(6, (*iterator)(0));
111 EXPECT_EQ(4, (*iterator)(1));
117 EXPECT_EQ(6, (*iterator)(0));
118 EXPECT_EQ(0, (*iterator)(1));
124 EXPECT_EQ(6, (*iterator)(0));
125 EXPECT_EQ(1, (*iterator)(1));
131 EXPECT_EQ(7, (*iterator)(0));
132 EXPECT_EQ(3, (*iterator)(1));
138 EXPECT_EQ(7, (*iterator)(0));
139 EXPECT_EQ(4, (*iterator)(1));
145 EXPECT_EQ(7, (*iterator)(0));
146 EXPECT_EQ(0, (*iterator)(1));
152 EXPECT_EQ(7, (*iterator)(0));
153 EXPECT_EQ(1, (*iterator)(1));
159 EXPECT_EQ(7, (*iterator)(0));
160 EXPECT_EQ(1, (*iterator)(1));
201 auto& layer = map.get(
"layer");
204 for (
long colIndex = 0; colIndex < layer.cols(); colIndex++) {
205 layer.col(colIndex).setConstant(static_cast<DataType>(colIndex));
208 std::cout <<
"(4,7) contains " << map.at(
"layer", {4, 7}) << std::endl;
213 auto checkCorrectValues = [](std::array<double, 4> given) {
216 for (
auto& value : given) {
217 if (std::abs(value - 1.0) < 1e-6) {
219 }
else if (std::abs(value - 2.0) < 1e-6) {
222 FAIL() <<
"Submap iterator returned unexpected value.";
225 EXPECT_EQ(countOnes, 2);
226 EXPECT_EQ(countTwos, 2);
229 std::array<double, 4> returnedSequence{};
230 returnedSequence.fill(0);
232 for (
size_t submapIndex = 0; submapIndex < 4; submapIndex++) {
233 returnedSequence.at(submapIndex) = map.at(
"layer", *iterator);
237 checkCorrectValues(returnedSequence);
241 returnedSequence.fill(0);
242 for (
size_t submapIndex = 0; submapIndex < 4; submapIndex++) {
243 if (submapIndex == 2) {
247 returnedSequence.at(submapIndex) = map.at(
"layer", *iterator);
250 checkCorrectValues(returnedSequence);
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
const Index & getSubmapIndex() const
bool move(const Position &position, std::vector< BufferRegion > &newRegions)
TEST(PositionFromIndex, Simple)