SubmapIteratorTest.cpp
Go to the documentation of this file.
00001 /*
00002  * SubmapIteratorTest.cpp
00003  *
00004  *  Created on: Sep 15, 2014
00005  *      Author: Péter Fankhauser
00006  *       Institute: ETH Zurich, Autonomous Systems Lab
00007  */
00008 
00009 #include "grid_map_core/iterators/SubmapIterator.hpp"
00010 #include "grid_map_core/GridMap.hpp"
00011 
00012 // Eigen
00013 #include <Eigen/Core>
00014 
00015 // gtest
00016 #include <gtest/gtest.h>
00017 
00018 // Limits
00019 #include <cfloat>
00020 
00021 // Vector
00022 #include <vector>
00023 
00024 using namespace std;
00025 using namespace Eigen;
00026 using namespace grid_map;
00027 
00028 TEST(SubmapIterator, Simple)
00029 {
00030   Eigen::Array2i submapTopLeftIndex(3, 1);
00031   Eigen::Array2i submapBufferSize(3, 2);
00032   Eigen::Array2i index;
00033   Eigen::Array2i submapIndex;
00034 
00035   vector<string> types;
00036   types.push_back("type");
00037   GridMap map(types);
00038   map.setGeometry(Array2d(8.1, 5.1), 1.0, Vector2d(0.0, 0.0)); // bufferSize(8, 5)
00039 
00040   SubmapIterator iterator(map, submapTopLeftIndex, submapBufferSize);
00041 
00042   EXPECT_FALSE(iterator.isPastEnd());
00043   EXPECT_EQ(submapTopLeftIndex(0), (*iterator)(0));
00044   EXPECT_EQ(submapTopLeftIndex(1), (*iterator)(1));
00045   EXPECT_EQ(0, iterator.getSubmapIndex()(0));
00046   EXPECT_EQ(0, iterator.getSubmapIndex()(1));
00047 
00048   ++iterator;
00049   EXPECT_FALSE(iterator.isPastEnd());
00050   EXPECT_EQ(3, (*iterator)(0));
00051   EXPECT_EQ(2, (*iterator)(1));
00052   EXPECT_EQ(0, iterator.getSubmapIndex()(0));
00053   EXPECT_EQ(1, iterator.getSubmapIndex()(1));
00054 
00055   ++iterator;
00056   EXPECT_FALSE(iterator.isPastEnd());
00057   EXPECT_EQ(4, (*iterator)(0));
00058   EXPECT_EQ(1, (*iterator)(1));
00059   EXPECT_EQ(1, iterator.getSubmapIndex()(0));
00060   EXPECT_EQ(0, iterator.getSubmapIndex()(1));
00061 
00062   ++iterator;
00063   EXPECT_FALSE(iterator.isPastEnd());
00064   EXPECT_EQ(4, (*iterator)(0));
00065   EXPECT_EQ(2, (*iterator)(1));
00066   EXPECT_EQ(1, iterator.getSubmapIndex()(0));
00067   EXPECT_EQ(1, iterator.getSubmapIndex()(1));
00068 
00069   ++iterator;
00070   EXPECT_FALSE(iterator.isPastEnd());
00071   EXPECT_EQ(5, (*iterator)(0));
00072   EXPECT_EQ(1, (*iterator)(1));
00073   EXPECT_EQ(2, iterator.getSubmapIndex()(0));
00074   EXPECT_EQ(0, iterator.getSubmapIndex()(1));
00075 
00076   ++iterator;
00077   EXPECT_FALSE(iterator.isPastEnd());
00078   EXPECT_EQ(5, (*iterator)(0));
00079   EXPECT_EQ(2, (*iterator)(1));
00080   EXPECT_EQ(2, iterator.getSubmapIndex()(0));
00081   EXPECT_EQ(1, iterator.getSubmapIndex()(1));
00082 
00083   ++iterator;
00084   EXPECT_TRUE(iterator.isPastEnd());
00085   EXPECT_EQ(5, (*iterator)(0));
00086   EXPECT_EQ(2, (*iterator)(1));
00087   EXPECT_EQ(2, iterator.getSubmapIndex()(0));
00088   EXPECT_EQ(1, iterator.getSubmapIndex()(1));
00089 }
00090 
00091 TEST(SubmapIterator, CircularBuffer)
00092 {
00093   Eigen::Array2i submapTopLeftIndex(6, 3);
00094   Eigen::Array2i submapBufferSize(2, 4);
00095   Eigen::Array2i index;
00096   Eigen::Array2i submapIndex;
00097 
00098   vector<string> types;
00099   types.push_back("type");
00100   GridMap map(types);
00101   map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
00102   map.move(Position(-3.0, -2.0)); // bufferStartIndex(3, 2)
00103 
00104   SubmapIterator iterator(map, submapTopLeftIndex, submapBufferSize);
00105 
00106   EXPECT_FALSE(iterator.isPastEnd());
00107   EXPECT_EQ(submapTopLeftIndex(0), (*iterator)(0));
00108   EXPECT_EQ(submapTopLeftIndex(1), (*iterator)(1));
00109   EXPECT_EQ(0, iterator.getSubmapIndex()(0));
00110   EXPECT_EQ(0, iterator.getSubmapIndex()(1));
00111 
00112   ++iterator;
00113   EXPECT_FALSE(iterator.isPastEnd());
00114   EXPECT_EQ(6, (*iterator)(0));
00115   EXPECT_EQ(4, (*iterator)(1));
00116   EXPECT_EQ(0, iterator.getSubmapIndex()(0));
00117   EXPECT_EQ(1, iterator.getSubmapIndex()(1));
00118 
00119   ++iterator;
00120   EXPECT_FALSE(iterator.isPastEnd());
00121   EXPECT_EQ(6, (*iterator)(0));
00122   EXPECT_EQ(0, (*iterator)(1));
00123   EXPECT_EQ(0, iterator.getSubmapIndex()(0));
00124   EXPECT_EQ(2, iterator.getSubmapIndex()(1));
00125 
00126   ++iterator;
00127   EXPECT_FALSE(iterator.isPastEnd());
00128   EXPECT_EQ(6, (*iterator)(0));
00129   EXPECT_EQ(1, (*iterator)(1));
00130   EXPECT_EQ(0, iterator.getSubmapIndex()(0));
00131   EXPECT_EQ(3, iterator.getSubmapIndex()(1));
00132 
00133   ++iterator;
00134   EXPECT_FALSE(iterator.isPastEnd());
00135   EXPECT_EQ(7, (*iterator)(0));
00136   EXPECT_EQ(3, (*iterator)(1));
00137   EXPECT_EQ(1, iterator.getSubmapIndex()(0));
00138   EXPECT_EQ(0, iterator.getSubmapIndex()(1));
00139 
00140   ++iterator;
00141   EXPECT_FALSE(iterator.isPastEnd());
00142   EXPECT_EQ(7, (*iterator)(0));
00143   EXPECT_EQ(4, (*iterator)(1));
00144   EXPECT_EQ(1, iterator.getSubmapIndex()(0));
00145   EXPECT_EQ(1, iterator.getSubmapIndex()(1));
00146 
00147   ++iterator;
00148   EXPECT_FALSE(iterator.isPastEnd());
00149   EXPECT_EQ(7, (*iterator)(0));
00150   EXPECT_EQ(0, (*iterator)(1));
00151   EXPECT_EQ(1, iterator.getSubmapIndex()(0));
00152   EXPECT_EQ(2, iterator.getSubmapIndex()(1));
00153 
00154   ++iterator;
00155   EXPECT_FALSE(iterator.isPastEnd());
00156   EXPECT_EQ(7, (*iterator)(0));
00157   EXPECT_EQ(1, (*iterator)(1));
00158   EXPECT_EQ(1, iterator.getSubmapIndex()(0));
00159   EXPECT_EQ(3, iterator.getSubmapIndex()(1));
00160 
00161   ++iterator;
00162   EXPECT_TRUE(iterator.isPastEnd());
00163   EXPECT_EQ(7, (*iterator)(0));
00164   EXPECT_EQ(1, (*iterator)(1));
00165   EXPECT_EQ(1, iterator.getSubmapIndex()(0));
00166   EXPECT_EQ(3, iterator.getSubmapIndex()(1));
00167 }


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Mon Oct 9 2017 03:09:16