28 #include <mrpt/examples_config.h>
30 MRPT_EXAMPLES_BASE_DIRECTORY +
string(
"maps_gridmap_benchmark/"));
68 cout <<
"Running test #1: getCell... ";
75 for (i = 0; i < N; i++)
77 p += gridMap->getCell(0, 0);
79 double T = tictac.
Tac();
80 cout <<
"-> " << 1e9 * T / N <<
" ns/iter. p=" << p
90 cout <<
"Running test #2: setCell... ";
96 for (i = 0; i < N; i++)
98 gridMap->setCell(0, 0, p);
100 double T = tictac.
Tac();
101 cout <<
"-> " << 1e9 * T / N <<
" ns/iter."
111 cout <<
"Running test #3: updateCell... ";
117 for (i = 0; i < N; i++)
119 gridMap->updateCell(0, 0, p);
121 double T = tictac.
Tac();
122 cout <<
"-> " << 1e9 * T / N <<
" ns/iter."
132 cout <<
"Running test #4: updateCell_fast... ";
140 unsigned theMapSize_x = gridMap->getSizeX();
145 for (i = 0; i < N; i++)
148 2, 2, logodd_obs, logodd_thres_occupied, theMapArray,
151 double T = tictac.
Tac();
152 cout <<
"-> " << 1e9 * T / N <<
" ns/iter."
159 CPose3D pose3D(0.21,0.34,0,-2);
164 gridMap->resizeGrid(-5,20,-15,15);
165 gridMap->insertObservation( &scan1, &pose3D );
166 gridMap->saveAsBitmapFile(
format(
"./gridmap_with_widening_%04i.png",i));
174 gridMap->insertionOptions.wideningBeamsWithDistance =
false;
176 cout <<
"Running test #5: Laser insert. w/o widen... ";
179 for (i = 0; i < N; i++)
191 gridMap->insertObservation(scan1, &pose3D);
193 double T = tictac.
Tac();
194 cout <<
"-> " << 1000 * T / N <<
" ms/iter, scans/sec:" << N / T
199 gridMap->insertObservation(scan1, &pose3D);
200 gridMap->saveAsBitmapFile(
"./gridmap_without_widening.png");
207 gridMap->insertionOptions.wideningBeamsWithDistance =
true;
209 cout <<
"Running test #6: Laser insert. widen... ";
212 for (i = 0; i < N; i++)
223 gridMap->insertObservation(scan1, &pose3D);
225 double T = tictac.
Tac();
226 cout <<
"-> " << 1000 * T / N <<
" ms/iter, scans/sec:" << N / T
231 gridMap->insertObservation(scan1, &pose3D);
232 gridMap->saveAsBitmapFile(
"./gridmap_with_widening.png");
240 cout <<
"Running test #7: Grid resize... ";
243 for (i = 0; i < N; i++)
245 *gridMap = gridMapCopy;
246 gridMap->resizeGrid(-30, 30, -40, 40);
248 double T = tictac.
Tac();
249 cout <<
"-> " << 1000 * T / N <<
" ms/iter" << endl;
258 *gridMap = gridMapCopy;
260 gridMap->insertObservation(scan1, &pose3D);
262 cout <<
"Running test #8: Likelihood... ";
266 for (i = 0; i < N; i++)
272 R += gridMap->computeObservationLikelihood(scan1, pose);
274 double T = tictac.
Tac();
275 cout <<
"-> " << 1000 * T / N <<
" ms/iter" << endl;
291 cout <<
"MRPT exception caught: " << e.what() << endl;
296 printf(
"Another exception!!");