grid_map_core: SlidingWindowIteratorTest.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * SlidingWindowIteratorTest.cpp
00003  *
00004  *  Created on: Aug 16, 2017
00005  *      Author: Péter Fankhauser
00006  *       Institute: ETH Zurich
00007  */
00008 
00009 #include "grid_map_core/iterators/SlidingWindowIterator.hpp"
00010 #include "grid_map_core/GridMap.hpp"
00011 
00012 #include <cfloat>
00013 #include <Eigen/Core>
00014 #include <gtest/gtest.h>
00015 #include <vector>
00016 
00017 using namespace std;
00018 using namespace grid_map;
00019 
00020 TEST(SlidingWindowIterator, WindowSize3Cutoff)
00021 {
00022   GridMap map;
00023   map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
00024   map.add("layer");
00025   map["layer"].setRandom();
00026 
00027   SlidingWindowIterator iterator(map, "layer", SlidingWindowIterator::EdgeHandling::CROP, 3);
00028   EXPECT_EQ(iterator.getData().rows(), 2);
00029   EXPECT_EQ(iterator.getData().cols(), 2);
00030   EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 2, 2)));
00031 
00032   ++iterator;
00033   EXPECT_EQ(iterator.getData().rows(), 3);
00034   EXPECT_EQ(iterator.getData().cols(), 2);
00035   EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 3, 2)));
00036 
00037   ++iterator;
00038   EXPECT_EQ(iterator.getData().rows(), 3);
00039   EXPECT_EQ(iterator.getData().cols(), 2);
00040   EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(1, 0, 3, 2)));
00041 
00042   for (; !iterator.isPastEnd(); ++iterator) {
00043     EXPECT_FALSE(iterator.isPastEnd());
00044     if ((*iterator == Index(3, 2)).all()) break;
00045   }
00046 
00047   EXPECT_EQ(iterator.getData().rows(), 3);
00048   EXPECT_EQ(iterator.getData().cols(), 3);
00049   EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(2, 1, 3, 3)));
00050 
00051   for (; !iterator.isPastEnd(); ++iterator) {
00052     EXPECT_FALSE(iterator.isPastEnd());
00053     if ((*iterator == Index(7, 4)).all()) break;
00054   }
00055 
00056   EXPECT_EQ(iterator.getData().rows(), 2);
00057   EXPECT_EQ(iterator.getData().cols(), 2);
00058   EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(6, 3, 2, 2)));
00059 
00060   ++iterator;
00061   EXPECT_TRUE(iterator.isPastEnd());
00062 }
00063 
00064 TEST(SlidingWindowIterator, WindowSize5)
00065 {
00066   GridMap map;
00067   map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
00068   map.add("layer");
00069   map["layer"].setRandom();
00070 
00071   SlidingWindowIterator iterator(map, "layer", SlidingWindowIterator::EdgeHandling::CROP, 5);
00072   EXPECT_EQ(iterator.getData().rows(), 3);
00073   EXPECT_EQ(iterator.getData().cols(), 3);
00074   EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 3, 3)));
00075 
00076   ++iterator;
00077   EXPECT_EQ(iterator.getData().rows(), 4);
00078   EXPECT_EQ(iterator.getData().cols(), 3);
00079   EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 4, 3)));
00080 
00081   ++iterator;
00082   EXPECT_EQ(iterator.getData().rows(), 5);
00083   EXPECT_EQ(iterator.getData().cols(), 3);
00084   EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 5, 3)));
00085 
00086   for (; !iterator.isPastEnd(); ++iterator) {
00087     EXPECT_FALSE(iterator.isPastEnd());
00088     if ((*iterator == Index(3, 2)).all()) break;
00089   }
00090 
00091   EXPECT_EQ(iterator.getData().rows(), 5);
00092   EXPECT_EQ(iterator.getData().cols(), 5);
00093   EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(1, 0, 5, 5)));
00094 
00095   for (; !iterator.isPastEnd(); ++iterator) {
00096     EXPECT_FALSE(iterator.isPastEnd());
00097     if ((*iterator == Index(7, 4)).all()) break;
00098   }
00099 
00100   EXPECT_EQ(iterator.getData().rows(), 3);
00101   EXPECT_EQ(iterator.getData().cols(), 3);
00102   EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(5, 2, 3, 3)));
00103 
00104   ++iterator;
00105   EXPECT_TRUE(iterator.isPastEnd());
00106 }
00107 
00108 TEST(SlidingWindowIterator, WindowSize3Inside)
00109 {
00110   GridMap map;
00111   map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
00112   map.add("layer");
00113   map["layer"].setRandom();
00114 
00115   SlidingWindowIterator iterator(map, "layer", SlidingWindowIterator::EdgeHandling::INSIDE, 3);
00116   EXPECT_EQ(iterator.getData().rows(), 3);
00117   EXPECT_EQ(iterator.getData().cols(), 3);
00118   EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 3, 3)));
00119 
00120   for (; !iterator.isPastEnd(); ++iterator) {
00121     EXPECT_FALSE(iterator.isPastEnd());
00122     if ((*iterator == Index(3, 2)).all()) break;
00123   }
00124 
00125   EXPECT_EQ(iterator.getData().rows(), 3);
00126   EXPECT_EQ(iterator.getData().cols(), 3);
00127   EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(2, 1, 3, 3)));
00128 
00129   for (; !iterator.isPastEnd(); ++iterator) {
00130     EXPECT_FALSE(iterator.isPastEnd());
00131     if ((*iterator == Index(6, 3)).all()) break;
00132   }
00133 
00134   EXPECT_EQ(iterator.getData().rows(), 3);
00135   EXPECT_EQ(iterator.getData().cols(), 3);
00136   EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(5, 2, 3, 3)));
00137 
00138   ++iterator;
00139   EXPECT_TRUE(iterator.isPastEnd());
00140 }