iterator_benchmark.cpp
Go to the documentation of this file.
00001 /*
00002  * grid_map_iterator_benchmark.hpp
00003  *
00004  *  Created on: Feb 15, 2016
00005  *     Authors: Christos Zalidis, Péter Fankhauser
00006  */
00007 
00008 #include <grid_map_core/grid_map_core.hpp>
00009 #include <chrono>
00010 #include <iostream>
00011 
00012 using namespace std;
00013 using namespace std::chrono;
00014 using namespace grid_map;
00015 
00016 #define duration(a) duration_cast<milliseconds>(a).count()
00017 typedef high_resolution_clock clk;
00018 
00022 void runGridMapIteratorVersion1(GridMap& map, const string& layer_from, const string& layer_to)
00023 {
00024   for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
00025     const float value_from = map.at(layer_from, *iterator);
00026     float& value_to = map.at(layer_to, *iterator);
00027     value_to = value_to > value_from ? value_to : value_from;
00028   }
00029 }
00030 
00034 void runGridMapIteratorVersion2(GridMap& map, const string& layer_from, const string& layer_to)
00035 {
00036   const auto& data_from = map[layer_from];
00037   auto& data_to = map[layer_to];
00038   for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
00039     const Index index(*iterator);
00040     const float value_from = data_from(index(0), index(1));
00041     float& value_to = data_to(index(0), index(1));
00042     value_to = value_to > value_from ? value_to : value_from;
00043   }
00044 }
00045 
00049 void runGridMapIteratorVersion3(GridMap& map, const string& layer_from, const string& layer_to)
00050 {
00051   const auto& data_from = map[layer_from];
00052   auto& data_to = map[layer_to];
00053   for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
00054     const size_t i = iterator.getLinearIndex();
00055     const float value_from = data_from(i);
00056     float& value_to = data_to(i);
00057     value_to = value_to > value_from ? value_to : value_from;
00058   }
00059 }
00060 
00065 void runEigenFunction(GridMap& map, const string& layer_from, const string& layer_to)
00066 {
00067   map[layer_to] = map[layer_to].cwiseMax(map[layer_from]);
00068 }
00069 
00073 void runCustomIndexIteration(GridMap& map, const string& layer_from, const string& layer_to)
00074 {
00075   const auto& data_from = map[layer_from];
00076   auto& data_to = map[layer_to];
00077   for (size_t j = 0; j < data_to.cols(); ++j) {
00078     for (size_t i = 0; i < data_to.rows(); ++i) {
00079       const float value_from = data_from(i, j);
00080       float& value_to = data_to(i, j);
00081       value_to = value_to > value_from ? value_to : value_from;
00082     }
00083   }
00084 }
00085 
00089 void runCustomLinearIndexIteration(GridMap& map, const string& layer_from, const string& layer_to)
00090 {
00091   const auto& data_from = map[layer_from];
00092   auto& data_to = map[layer_to];
00093   for (size_t i = 0; i < data_to.size(); ++i) {
00094     data_to(i) = data_to(i) > data_from(i) ? data_to(i) : data_from(i);
00095   }
00096 }
00097 
00098 int main(int argc, char* argv[])
00099 {
00100   GridMap map;
00101   map.setGeometry(Length(20.0, 20.0), 0.004, Position(0.0, 0.0));
00102   map.add("random");
00103   map["random"].setRandom();
00104   map.add("layer1", 0.0);
00105   map.add("layer2", 0.0);
00106   map.add("layer3", 0.0);
00107   map.add("layer4", 0.0);
00108   map.add("layer5", 0.0);
00109   map.add("layer6", 0.0);
00110 
00111   cout << "Results for iteration over " << map.getSize()(0) << " x " << map.getSize()(1) << " (" << map.getSize().prod() << ") grid cells." << endl;
00112   cout << "=========================================" << endl;
00113 
00114   clk::time_point t1 = clk::now();
00115   runGridMapIteratorVersion1(map, "random", "layer1");
00116   clk::time_point t2 = clk::now();
00117   cout << "Duration grid map iterator (convenient use): " << duration(t2 - t1) << " ms" << endl;
00118 
00119   t1 = clk::now();
00120   runGridMapIteratorVersion2(map, "random", "layer2");
00121   t2 = clk::now();
00122   cout << "Duration grid map iterator (direct access to data layers): " << duration(t2 - t1) << " ms" << endl;
00123 
00124   t1 = clk::now();
00125   runGridMapIteratorVersion3(map, "random", "layer3");
00126   t2 = clk::now();
00127   cout << "Duration grid map iterator (linear index): " << duration(t2 - t1) << " ms" << endl;
00128 
00129   t1 = clk::now();
00130   runEigenFunction(map, "random", "layer4");
00131   t2 = clk::now();
00132   cout << "Duration Eigen function: " << duration(t2 - t1) << " ms" <<  endl;
00133 
00134   t1 = clk::now();
00135   runCustomIndexIteration(map, "random", "layer5");
00136   t2 = clk::now();
00137   cout << "Duration custom index iteration: " << duration(t2 - t1) << " ms" <<  endl;
00138 
00139   t1 = clk::now();
00140   runCustomLinearIndexIteration(map, "random", "layer6");
00141   t2 = clk::now();
00142   cout << "Duration custom linear index iteration: " << duration(t2 - t1) << " ms" <<  endl;
00143 
00144   return 0;
00145 }


grid_map_demos
Author(s): Péter Fankhauser
autogenerated on Tue Jul 9 2019 05:06:58