MRPT  1.9.9
test.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <mrpt/random.h>
11 #include <mrpt/maps/CMultiMetricMap.h>
12 #include <mrpt/obs/CObservation2DRangeScan.h>
13 #include <mrpt/obs/CRawlog.h>
14 #include <mrpt/config/CConfigFile.h>
15 #include <mrpt/system/CTicTac.h>
16 #include <iostream>
17 
18 using namespace mrpt;
19 using namespace mrpt::maps;
20 using namespace mrpt::obs;
21 using namespace mrpt::random;
22 using namespace mrpt::poses;
23 using namespace mrpt::system;
24 using namespace std;
25 
26 #include <mrpt/examples_config.h>
27 string myDataDir(
28  MRPT_EXAMPLES_BASE_DIRECTORY + string("maps_gridmap_benchmark/"));
29 
30 // Default .ini file:
31 string iniFile(myDataDir + string("benchmark-options.ini"));
32 
33 #define SCANS_SIZE 361
34 
35 // Two laser scans:
36 float SCAN_RANGES_1[] = {
37  0.910f, 0.900f, 0.910f, 0.900f, 0.900f, 0.890f, 0.890f, 0.880f,
38  0.890f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.870f,
39  0.880f, 0.870f, 0.870f, 0.870f, 0.880f, 0.880f, 0.880f, 0.880f,
40  0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f,
41  0.880f, 0.880f, 0.880f, 0.880f, 0.890f, 0.880f, 0.880f, 0.880f,
42  0.890f, 0.880f, 0.890f, 0.890f, 0.880f, 0.890f, 0.890f, 0.880f,
43  0.890f, 0.890f, 0.890f, 0.890f, 0.890f, 0.890f, 0.900f, 0.900f,
44  0.900f, 0.900f, 0.900f, 0.910f, 0.910f, 0.910f, 0.910f, 0.920f,
45  0.920f, 0.920f, 0.920f, 0.920f, 0.930f, 0.930f, 0.930f, 0.930f,
46  0.940f, 0.940f, 0.950f, 0.950f, 0.950f, 0.950f, 0.960f, 0.960f,
47  0.970f, 0.970f, 0.970f, 0.980f, 0.980f, 0.990f, 1.000f, 1.000f,
48  1.000f, 1.010f, 1.010f, 1.020f, 1.030f, 1.030f, 1.030f, 1.040f,
49  1.050f, 1.060f, 1.050f, 1.060f, 1.070f, 1.070f, 1.080f, 1.080f,
50  1.090f, 1.100f, 1.110f, 1.120f, 1.120f, 1.130f, 1.140f, 1.140f,
51  1.160f, 1.170f, 1.180f, 1.180f, 1.190f, 1.200f, 1.220f, 1.220f,
52  1.230f, 1.230f, 1.240f, 1.250f, 1.270f, 1.280f, 1.290f, 1.300f,
53  1.320f, 1.320f, 1.350f, 1.360f, 1.370f, 1.390f, 1.410f, 1.410f,
54  1.420f, 1.430f, 1.450f, 1.470f, 1.490f, 1.500f, 1.520f, 1.530f,
55  1.560f, 1.580f, 1.600f, 1.620f, 1.650f, 1.670f, 1.700f, 1.730f,
56  1.750f, 1.780f, 1.800f, 1.830f, 1.850f, 1.880f, 1.910f, 1.940f,
57  1.980f, 2.010f, 2.060f, 2.090f, 2.130f, 2.180f, 2.220f, 2.250f,
58  2.300f, 2.350f, 2.410f, 2.460f, 2.520f, 2.570f, 2.640f, 2.700f,
59  2.780f, 2.850f, 2.930f, 3.010f, 3.100f, 3.200f, 3.300f, 3.390f,
60  3.500f, 3.620f, 3.770f, 3.920f, 4.070f, 4.230f, 4.430f, 4.610f,
61  4.820f, 5.040f, 5.290f, 5.520f, 8.970f, 8.960f, 8.950f, 8.930f,
62  8.940f, 8.930f, 9.050f, 9.970f, 9.960f, 10.110f, 13.960f, 18.870f,
63  19.290f, 81.910f, 20.890f, 48.750f, 48.840f, 48.840f, 19.970f, 19.980f,
64  19.990f, 15.410f, 20.010f, 19.740f, 17.650f, 17.400f, 14.360f, 12.860f,
65  11.260f, 11.230f, 8.550f, 8.630f, 9.120f, 9.120f, 8.670f, 8.570f,
66  7.230f, 7.080f, 7.040f, 6.980f, 6.970f, 5.260f, 5.030f, 4.830f,
67  4.620f, 4.440f, 4.390f, 4.410f, 4.410f, 4.410f, 4.430f, 4.440f,
68  4.460f, 4.460f, 4.490f, 4.510f, 4.540f, 3.970f, 3.820f, 3.730f,
69  3.640f, 3.550f, 3.460f, 3.400f, 3.320f, 3.300f, 3.320f, 3.320f,
70  3.340f, 2.790f, 2.640f, 2.600f, 2.570f, 2.540f, 2.530f, 2.510f,
71  2.490f, 2.490f, 2.480f, 2.470f, 2.460f, 2.460f, 2.460f, 2.450f,
72  2.450f, 2.450f, 2.460f, 2.460f, 2.470f, 2.480f, 2.490f, 2.490f,
73  2.520f, 2.510f, 2.550f, 2.570f, 2.610f, 2.640f, 2.980f, 3.040f,
74  3.010f, 2.980f, 2.940f, 2.920f, 2.890f, 2.870f, 2.830f, 2.810f,
75  2.780f, 2.760f, 2.740f, 2.720f, 2.690f, 2.670f, 2.650f, 2.630f,
76  2.620f, 2.610f, 2.590f, 2.560f, 2.550f, 2.530f, 2.510f, 2.500f,
77  2.480f, 2.460f, 2.450f, 2.430f, 2.420f, 2.400f, 2.390f, 2.380f,
78  2.360f, 2.350f, 2.340f, 2.330f, 2.310f, 2.300f, 2.290f, 2.280f,
79  2.270f, 2.260f, 2.250f, 2.240f, 2.230f, 2.230f, 2.220f, 2.210f,
80  2.200f, 2.190f, 2.180f, 2.170f, 1.320f, 1.140f, 1.130f, 1.130f,
81  1.120f, 1.120f, 1.110f, 1.110f, 1.110f, 1.110f, 1.100f, 1.110f,
82  1.100f};
83 char SCAN_VALID_1[] = {
84  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
85  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
86  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
87  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
88  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
89  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
90  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
91  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
92  1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
93  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
94  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
95  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
96  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
97  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
98  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
99 
100 // float SCAN_RANGES_2[] =
101 // {0.720f,0.720f,0.720f,0.720f,0.720f,0.720f,0.710f,0.720f,0.710f,0.710f,0.710f,0.710f,0.710f,0.710f,0.710f,0.710f,0.710f,0.710f,0.710f,0.710f,0.720f,0.720f,0.720f,0.720f,0.730f,0.730f,0.730f,0.730f,0.730f,0.730f,0.730f,0.740f,0.740f,0.740f,0.740f,0.740f,0.740f,0.750f,0.750f,0.750f,0.750f,0.750f,0.750f,0.750f,0.750f,0.760f,0.760f,0.760f,0.760f,0.760f,0.760f,0.760f,0.760f,0.770f,0.770f,0.770f,0.770f,0.780f,0.780f,0.780f,0.790f,0.790f,0.800f,0.800f,0.800f,0.800f,0.800f,0.800f,0.810f,0.810f,0.820f,0.820f,0.830f,0.830f,0.840f,0.840f,0.850f,0.850f,0.860f,0.860f,0.860f,0.870f,0.870f,0.880f,0.890f,0.890f,0.900f,0.900f,0.910f,0.920f,0.930f,0.930f,0.940f,0.940f,0.940f,0.950f,0.960f,0.960f,0.970f,0.980f,0.990f,1.000f,1.010f,1.020f,1.030f,1.040f,1.050f,1.060f,1.070f,1.080f,1.080f,1.100f,1.100f,1.120f,1.120f,1.140f,1.140f,1.170f,1.160f,1.180f,1.190f,1.210f,1.220f,1.240f,1.250f,1.280f,1.290f,1.300f,1.320f,1.340f,1.350f,1.380f,1.390f,1.420f,1.440f,1.460f,1.470f,1.500f,1.520f,1.550f,1.570f,1.600f,1.630f,1.670f,1.690f,1.730f,1.760f,1.790f,1.820f,1.870f,1.900f,1.940f,1.970f,2.030f,2.080f,2.130f,2.170f,2.230f,2.280f,2.340f,2.400f,2.490f,2.550f,2.630f,2.700f,2.810f,2.880f,3.010f,3.090f,3.240f,3.340f,3.500f,3.620f,3.810f,3.950f,4.180f,4.340f,4.620f,8.170f,8.140f,8.150f,8.120f,8.110f,8.100f,8.100f,8.300f,9.040f,9.130f,9.130f,13.030f,18.050f,19.150f,81.910f,20.070f,47.980f,48.040f,48.030f,19.140f,19.180f,19.180f,19.190f,14.550f,19.210f,16.850f,16.840f,7.800f,7.770f,7.770f,7.750f,7.770f,7.760f,7.780f,7.760f,8.320f,8.350f,8.350f,8.090f,7.720f,7.730f,6.430f,6.360f,6.290f,6.260f,6.230f,6.220f,6.160f,5.800f,4.510f,4.410f,4.240f,4.140f,4.000f,3.910f,3.790f,3.680f,3.660f,3.680f,3.680f,3.700f,3.710f,3.730f,3.730f,3.760f,3.770f,3.790f,3.820f,3.850f,3.900f,3.940f,3.980f,3.250f,3.180f,3.140f,3.070f,3.030f,2.970f,2.930f,2.880f,2.850f,2.790f,2.760f,2.710f,2.680f,2.660f,2.670f,2.690f,2.710f,2.720f,2.740f,2.760f,2.770f,2.780f,2.800f,2.170f,2.120f,2.090f,2.060f,2.020f,2.010f,1.990f,1.980f,1.970f,1.960f,1.950f,1.950f,1.940f,1.940f,1.950f,1.940f,1.940f,1.950f,1.930f,1.940f,1.940f,1.940f,1.940f,1.940f,1.950f,1.960f,1.960f,1.980f,1.980f,2.000f,2.010f,2.030f,2.060f,2.090f,2.120f,2.190f,2.560f,2.540f,2.530f,2.520f,2.500f,2.490f,2.470f,2.460f,2.450f,2.440f,2.420f,2.410f,2.400f,2.390f,2.380f,2.370f,2.360f,2.350f,2.340f,2.340f,2.330f,2.320f,2.310f,2.300f,2.290f,2.290f,2.290f,2.280f,2.270f,2.260f,2.260f,2.250f,2.240f,2.240f,2.230f,2.230f,2.220f,2.220f,2.210f,2.210f,2.200f,2.200f,2.190f,2.190f,2.190f,2.180f,2.180f,2.170f,2.170f,2.170f,2.160f,2.160f};
102 // char SCAN_VALID_2[] =
103 // {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};
104 
105 // ------------------------------------------------------
106 // BenchmarkGridmaps
107 // ------------------------------------------------------
108 void BenchmarkGridmaps()
109 {
110  getRandomGenerator().randomize(333);
111 
112  CMultiMetricMap metricMap;
113  TSetOfMetricMapInitializers mapInit;
114 
115  // Create gridmap:
116  mapInit.loadFromConfigFile(
117  mrpt::config::CConfigFile(iniFile), "METRIC_MAPS");
118  metricMap.setListOfMaps(&mapInit);
119 
120  // prepare the laser scan:
121  CObservation2DRangeScan scan1;
122  scan1.aperture = M_PIf;
123  scan1.rightToLeft = true;
124  ASSERT_(sizeof(SCAN_RANGES_1) == sizeof(float) * SCANS_SIZE);
125  scan1.loadFromVectors(SCANS_SIZE, SCAN_RANGES_1, SCAN_VALID_1);
126 
127 #if 1
128  CRawlog rawlog;
129  rawlog.loadFromRawLogFile(
130  "/Trabajo/Code/MRPT/share/mrpt/datasets/2006-01ENE-21-SENA_Telecom "
131  "Faculty_one_loop_only.rawlog");
132  scan1 = *rawlog.getAsObservations(400)
133  ->getObservationByClass<CObservation2DRangeScan>();
134 #endif
135 
136  ASSERT_(metricMap.m_gridMaps.size());
137  COccupancyGridMap2D::Ptr gridMap = metricMap.m_gridMaps[0];
138  COccupancyGridMap2D gridMapCopy(*gridMap);
139 
140  int i, N;
141  CTicTac tictac;
142 
143  // test 1: getcell
144  // ----------------------------------------
145  if (1)
146  {
147  N = 10000000;
148 
149  cout << "Running test #1: getCell... ";
150  cout.flush();
151 
152  // COccupancyGridMap2D::cellType cell;
153  float p = 0;
154 
155  tictac.Tic();
156  for (i = 0; i < N; i++)
157  {
158  p += gridMap->getCell(0, 0);
159  }
160  double T = tictac.Tac();
161  cout << "-> " << 1e9 * T / N << " ns/iter. p=" << p
162  << endl; // the "p" is to avoid optimizing out the entire loop!
163  }
164 
165  // test 2: setcell
166  // ----------------------------------------
167  if (1)
168  {
169  N = 10000000;
170 
171  cout << "Running test #2: setCell... ";
172  cout.flush();
173 
174  float p = 0.8f;
175 
176  tictac.Tic();
177  for (i = 0; i < N; i++)
178  {
179  gridMap->setCell(0, 0, p);
180  }
181  double T = tictac.Tac();
182  cout << "-> " << 1e9 * T / N << " ns/iter."
183  << endl; // the "p" is to avoid optimizing out the entire loop!
184  }
185 
186  // test 3: updateCell
187  // ----------------------------------------
188  if (1)
189  {
190  N = 1000000;
191 
192  cout << "Running test #3: updateCell... ";
193  cout.flush();
194 
195  float p = 0.57f;
196 
197  tictac.Tic();
198  for (i = 0; i < N; i++)
199  {
200  gridMap->updateCell(0, 0, p);
201  }
202  double T = tictac.Tac();
203  cout << "-> " << 1e9 * T / N << " ns/iter."
204  << endl; // the "p" is to avoid optimizing out the entire loop!
205  }
206 
207  // test 4: updateCell_fast
208  // ----------------------------------------
209  if (1)
210  {
211  N = 10000000;
212 
213  cout << "Running test #4: updateCell_fast... ";
214  cout.flush();
215 
216  float p = 0.57f;
217  COccupancyGridMap2D::cellType logodd_obs = COccupancyGridMap2D::p2l(p);
218  // float p_1 = 1-p;
219 
220  COccupancyGridMap2D::cellType* theMapArray = gridMap->getRow(0);
221  unsigned theMapSize_x = gridMap->getSizeX();
222  COccupancyGridMap2D::cellType logodd_thres_occupied =
223  COccupancyGridMap2D::OCCGRID_CELLTYPE_MIN + logodd_obs;
224 
225  tictac.Tic();
226  for (i = 0; i < N; i++)
227  {
228  COccupancyGridMap2D::updateCell_fast_occupied(
229  2, 2, logodd_obs, logodd_thres_occupied, theMapArray,
230  theMapSize_x);
231  }
232  double T = tictac.Tac();
233  cout << "-> " << 1e9 * T / N << " ns/iter."
234  << endl; // the "p" is to avoid optimizing out the entire loop!
235  }
236 
237 #if 0
238  for (i=50;i<51;i++)
239  {
240  CPose3D pose3D(0.21,0.34,0,-2);
241  //scan1.validRange.assign(scan1.validRange.size(), false);
242  //scan1.validRange[i]=true;
243 
244  gridMap->clear();
245  gridMap->resizeGrid(-5,20,-15,15);
246  gridMap->insertObservation( &scan1, &pose3D );
247  gridMap->saveAsBitmapFile(format("./gridmap_with_widening_%04i.png",i));
248  }
249 #endif
250 
251  // test 5: Laser insertion
252  // ----------------------------------------
253  if (1)
254  {
255  gridMap->insertionOptions.wideningBeamsWithDistance = false;
256  N = 3000;
257  cout << "Running test #5: Laser insert. w/o widen... ";
258  cout.flush();
259  tictac.Tic();
260  for (i = 0; i < N; i++)
261  {
262 #if 1
263  CPose2D pose(
264  getRandomGenerator().drawUniform(-1.0, 1.0),
265  getRandomGenerator().drawUniform(-1.0, 1.0),
266  getRandomGenerator().drawUniform(-M_PI, M_PI));
267  CPose3D pose3D(pose);
268 #else
269  CPose3D pose3D;
270 #endif
271 
272  gridMap->insertObservation(&scan1, &pose3D);
273  }
274  double T = tictac.Tac();
275  cout << "-> " << 1000 * T / N << " ms/iter, scans/sec:" << N / T
276  << endl;
277 
278  CPose3D pose3D;
279  gridMap->clear();
280  gridMap->insertObservation(&scan1, &pose3D);
281  gridMap->saveAsBitmapFile("./gridmap_without_widening.png");
282  }
283 
284  // test 6: Laser insertion without widening
285  // --------------------------------------------------
286  if (1)
287  {
288  gridMap->insertionOptions.wideningBeamsWithDistance = true;
289  N = 3000;
290  cout << "Running test #6: Laser insert. widen... ";
291  cout.flush();
292  tictac.Tic();
293  for (i = 0; i < N; i++)
294  {
295 #if 1
296  CPose2D pose(
297  getRandomGenerator().drawUniform(-1.0, 1.0),
298  getRandomGenerator().drawUniform(-1.0, 1.0),
299  getRandomGenerator().drawUniform(-M_PI, M_PI));
300  CPose3D pose3D(pose);
301 #else
302  CPose3D pose3D;
303 #endif
304  gridMap->insertObservation(&scan1, &pose3D);
305  }
306  double T = tictac.Tac();
307  cout << "-> " << 1000 * T / N << " ms/iter, scans/sec:" << N / T
308  << endl;
309 
310  CPose3D pose3D;
311  gridMap->clear();
312  gridMap->insertObservation(&scan1, &pose3D);
313  gridMap->saveAsBitmapFile("./gridmap_with_widening.png");
314  }
315 
316  // test 7: Grid resize
317  // ----------------------------------------
318  if (1)
319  {
320  N = 400;
321  cout << "Running test #7: Grid resize... ";
322  cout.flush();
323  tictac.Tic();
324  for (i = 0; i < N; i++)
325  {
326  *gridMap = gridMapCopy;
327  gridMap->resizeGrid(-30, 30, -40, 40);
328  }
329  double T = tictac.Tac();
330  cout << "-> " << 1000 * T / N << " ms/iter" << endl;
331  }
332 
333  // test 8: Likelihood computation
334  // ----------------------------------------
335  if (1)
336  {
337  N = 5000;
338 
339  *gridMap = gridMapCopy;
340  CPose3D pose3D(0, 0, 0);
341  gridMap->insertObservation(&scan1, &pose3D);
342 
343  cout << "Running test #8: Likelihood... ";
344  cout.flush();
345  double R = 0;
346  tictac.Tic();
347  for (i = 0; i < N; i++)
348  {
349  CPose2D pose(
350  getRandomGenerator().drawUniform(-1.0, 1.0),
351  getRandomGenerator().drawUniform(-1.0, 1.0),
352  getRandomGenerator().drawUniform(-M_PI, M_PI));
353  R += gridMap->computeObservationLikelihood(&scan1, pose);
354  }
355  double T = tictac.Tac();
356  cout << "-> " << 1000 * T / N << " ms/iter" << endl;
357  }
358 }
359 
360 int main(int argc, char** argv)
361 {
362  try
363  {
364  // optional argument: a different ini file
365  if (argc > 1) iniFile = string(argv[1]);
366 
367  BenchmarkGridmaps();
368  return 0;
369  }
370  catch (exception& e)
371  {
372  cout << "MRPT exception caught: " << e.what() << endl;
373  return -1;
374  }
375  catch (...)
376  {
377  printf("Another exception!!");
378  return -1;
379  }
380 }
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:17
mrpt::system::CTicTac::Tac
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:90
mrpt::maps::COccupancyGridMap2D::cellType
int16_t cellType
The type of the map cells:
Definition: COccupancyGridMap2D.h:75
mrpt::random::CRandomGenerator::randomize
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
Definition: RandomGenerator.cpp:32
mrpt::config::CConfigFile
This class allows loading and storing values and vectors of different types from ".ini" files easily.
Definition: config/CConfigFile.h:31
mrpt::system::CTicTac
A high-performance stopwatch, with typical resolution of nanoseconds.
Definition: system/CTicTac.h:17
myDataDir
std::string myDataDir
Definition: vision_stereo_rectify/test.cpp:23
std
STL namespace.
CRawlog.h
mrpt::maps::TSetOfMetricMapInitializers
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Definition: TMetricMapInitializer.h:89
SCANS_SIZE
#define SCANS_SIZE
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
mrpt::obs::CRawlog::loadFromRawLogFile
bool loadFromRawLogFile(const std::string &fileName, bool non_obs_objects_are_legal=false)
Load the contents from a file containing one of these possibilities:
Definition: CRawlog.cpp:190
mrpt::maps::TSetOfMetricMapInitializers::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &sectionName) override
Loads the configuration for the set of internal maps from a textual definition in an INI-like file...
Definition: TMetricMapInitializer.cpp:69
std::shared_ptr< COccupancyGridMap2D >
main
int main()
Definition: vision_stereo_rectify/test.cpp:78
mrpt::obs::CRawlog
This class stores a rawlog (robotic datasets) in one of two possible formats:
Definition: CRawlog.h:66
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:38
M_PIf
#define M_PIf
Definition: common.h:61
iniFile
string iniFile(myDataDir+string("benchmark-options.ini"))
mrpt::maps::CMultiMetricMap::m_gridMaps
ProxyFilterContainerByClass< mrpt::maps::COccupancyGridMap2D::Ptr, TListMaps > m_gridMaps
STL-like proxy to access this kind of maps in maps.
Definition: CMultiMetricMap.h:363
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
mrpt::obs::CRawlog::getAsObservations
CSensoryFrame::Ptr getAsObservations(size_t index) const
Returns the i&#39;th element in the sequence, as being an action, where index=0 is the first object...
Definition: CRawlog.cpp:149
CObservation2DRangeScan.h
SCAN_RANGES_1
float SCAN_RANGES_1[]
Definition: vision_stereo_rectify/test.cpp:36
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: COccupancyGridMap2D.h:60
mrpt::maps::COccupancyGridMap2D::OCCGRID_CELLTYPE_MIN
static const cellType OCCGRID_CELLTYPE_MIN
Discrete to float conversion factors: The min/max values of the integer cell type, eg.
Definition: COccupancyGridMap2D.h:81
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
CMultiMetricMap.h
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:56
mrpt::maps::COccupancyGridMap2D::p2l
static cellType p2l(const float p)
Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the val...
Definition: COccupancyGridMap2D.h:361
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
R
const float R
Definition: CKinematicChain.cpp:138
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
mrpt::maps::CMultiMetricMap::setListOfMaps
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers *initializers)
Sets the list of internal map according to the passed list of map initializers (Current maps&#39; content...
Definition: CMultiMetricMap.cpp:219
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::obs::CObservation2DRangeScan::aperture
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
Definition: CObservation2DRangeScan.h:125
mrpt::obs::CObservation2DRangeScan::loadFromVectors
void loadFromVectors(size_t nRays, const float *scanRanges, const char *scanValidity)
Definition: CObservation2DRangeScan.cpp:552
SCAN_VALID_1
char SCAN_VALID_1[]
Definition: vision_stereo_rectify/test.cpp:83
mrpt::system::CTicTac::Tic
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:79
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: CMultiMetricMap.h:139
BenchmarkGridmaps
void BenchmarkGridmaps()
Definition: vision_stereo_rectify/test.cpp:108
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:19
p
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::obs::CObservation2DRangeScan::rightToLeft
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
Definition: CObservation2DRangeScan.h:127
mrpt::maps::CLogOddsGridMap2D< int16_t >::updateCell_fast_occupied
static void updateCell_fast_occupied(const unsigned x, const unsigned y, const cell_t logodd_obs, const cell_t thres, cell_t *mapArray, const unsigned _size_x)
Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor upd...
Definition: CLogOddsGridMap2D.h:72


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020