26 #include <mrpt/examples_config.h>
28 MRPT_EXAMPLES_BASE_DIRECTORY +
string(
"maps_gridmap_benchmark/"));
33 #define SCANS_SIZE 361
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,
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};
130 "/Trabajo/Code/MRPT/share/mrpt/datasets/2006-01ENE-21-SENA_Telecom "
131 "Faculty_one_loop_only.rawlog");
149 cout <<
"Running test #1: getCell... ";
156 for (i = 0; i < N; i++)
158 p += gridMap->getCell(0, 0);
160 double T = tictac.
Tac();
161 cout <<
"-> " << 1e9 * T / N <<
" ns/iter. p=" <<
p
171 cout <<
"Running test #2: setCell... ";
177 for (i = 0; i < N; i++)
179 gridMap->setCell(0, 0,
p);
181 double T = tictac.
Tac();
182 cout <<
"-> " << 1e9 * T / N <<
" ns/iter."
192 cout <<
"Running test #3: updateCell... ";
198 for (i = 0; i < N; i++)
200 gridMap->updateCell(0, 0,
p);
202 double T = tictac.
Tac();
203 cout <<
"-> " << 1e9 * T / N <<
" ns/iter."
213 cout <<
"Running test #4: updateCell_fast... ";
221 unsigned theMapSize_x = gridMap->getSizeX();
226 for (i = 0; i < N; i++)
229 2, 2, logodd_obs, logodd_thres_occupied, theMapArray,
232 double T = tictac.
Tac();
233 cout <<
"-> " << 1e9 * T / N <<
" ns/iter."
240 CPose3D pose3D(0.21,0.34,0,-2);
245 gridMap->resizeGrid(-5,20,-15,15);
246 gridMap->insertObservation( &scan1, &pose3D );
247 gridMap->saveAsBitmapFile(
format(
"./gridmap_with_widening_%04i.png",i));
255 gridMap->insertionOptions.wideningBeamsWithDistance =
false;
257 cout <<
"Running test #5: Laser insert. w/o widen... ";
260 for (i = 0; i < N; i++)
272 gridMap->insertObservation(&scan1, &pose3D);
274 double T = tictac.
Tac();
275 cout <<
"-> " << 1000 * T / N <<
" ms/iter, scans/sec:" << N / T
280 gridMap->insertObservation(&scan1, &pose3D);
281 gridMap->saveAsBitmapFile(
"./gridmap_without_widening.png");
288 gridMap->insertionOptions.wideningBeamsWithDistance =
true;
290 cout <<
"Running test #6: Laser insert. widen... ";
293 for (i = 0; i < N; i++)
304 gridMap->insertObservation(&scan1, &pose3D);
306 double T = tictac.
Tac();
307 cout <<
"-> " << 1000 * T / N <<
" ms/iter, scans/sec:" << N / T
312 gridMap->insertObservation(&scan1, &pose3D);
313 gridMap->saveAsBitmapFile(
"./gridmap_with_widening.png");
321 cout <<
"Running test #7: Grid resize... ";
324 for (i = 0; i < N; i++)
326 *gridMap = gridMapCopy;
327 gridMap->resizeGrid(-30, 30, -40, 40);
329 double T = tictac.
Tac();
330 cout <<
"-> " << 1000 * T / N <<
" ms/iter" << endl;
339 *gridMap = gridMapCopy;
341 gridMap->insertObservation(&scan1, &pose3D);
343 cout <<
"Running test #8: Likelihood... ";
347 for (i = 0; i < N; i++)
353 R += gridMap->computeObservationLikelihood(&scan1, pose);
355 double T = tictac.
Tac();
356 cout <<
"-> " << 1000 * T / N <<
" ms/iter" << endl;
360 int main(
int argc,
char** argv)
372 cout <<
"MRPT exception caught: " << e.what() << endl;
377 printf(
"Another exception!!");