Example: maps_gridmap_benchmark

C++ example source code:

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <mrpt/config/CConfigFile.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/obs/stock_observations.h>
#include <mrpt/random.h>
#include <mrpt/system/CTicTac.h>

#include <iostream>

using namespace mrpt;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::random;
using namespace mrpt::poses;
using namespace mrpt::system;
using namespace std;

#include <mrpt/examples_config.h>
string myDataDir(MRPT_EXAMPLES_BASE_DIRECTORY + string("maps_gridmap_benchmark/"));

// Default .ini file:
string iniFile(myDataDir + string("benchmark-options.ini"));

// ------------------------------------------------------
//              BenchmarkGridmaps
// ------------------------------------------------------
void BenchmarkGridmaps()
{
  getRandomGenerator().randomize(333);

  CMultiMetricMap metricMap;
  TSetOfMetricMapInitializers mapInit;

  // Create gridmap:
  mapInit.loadFromConfigFile(mrpt::config::CConfigFile(iniFile), "METRIC_MAPS");
  metricMap.setListOfMaps(mapInit);

  // prepare the laser scan:
  CObservation2DRangeScan scan1;
  stock_observations::example2DRangeScan(scan1);

  COccupancyGridMap2D::Ptr gridMap = metricMap.mapByClass<COccupancyGridMap2D>();
  ASSERT_(gridMap);
  COccupancyGridMap2D gridMapCopy(*gridMap);

  int i, N;
  CTicTac tictac;

  // test 1: getcell
  // ----------------------------------------
  if (true)
  {
    N = 10000000;

    cout << "Running test #1: getCell... ";
    cout.flush();

    // COccupancyGridMap2D::cellType    cell;
    float p = 0;

    tictac.Tic();
    for (i = 0; i < N; i++)
    {
      p += gridMap->getCell(0, 0);
    }
    double T = tictac.Tac();
    cout << "-> " << 1e9 * T / N << " ns/iter. p=" << p
         << endl;  // the "p" is to avoid optimizing out the entire loop!
  }

  // test 2: setcell
  // ----------------------------------------
  if (true)
  {
    N = 10000000;

    cout << "Running test #2: setCell... ";
    cout.flush();

    float p = 0.8f;

    tictac.Tic();
    for (i = 0; i < N; i++)
    {
      gridMap->setCell(0, 0, p);
    }
    double T = tictac.Tac();
    cout << "-> " << 1e9 * T / N << " ns/iter."
         << endl;  // the "p" is to avoid optimizing out the entire loop!
  }

  // test 3: updateCell
  // ----------------------------------------
  if (true)
  {
    N = 1000000;

    cout << "Running test #3: updateCell... ";
    cout.flush();

    float p = 0.57f;

    tictac.Tic();
    for (i = 0; i < N; i++)
    {
      gridMap->updateCell(0, 0, p);
    }
    double T = tictac.Tac();
    cout << "-> " << 1e9 * T / N << " ns/iter."
         << endl;  // the "p" is to avoid optimizing out the entire loop!
  }

  // test 4: updateCell_fast
  // ----------------------------------------
  if (true)
  {
    N = 10000000;

    cout << "Running test #4: updateCell_fast... ";
    cout.flush();

    float p = 0.57f;
    COccupancyGridMap2D::cellType logodd_obs = COccupancyGridMap2D::p2l(p);
    // float   p_1 = 1-p;

    COccupancyGridMap2D::cellType* theMapArray = gridMap->getRow(0);
    unsigned theMapSize_x = gridMap->getSizeX();
    COccupancyGridMap2D::cellType logodd_thres_occupied =
        COccupancyGridMap2D::OCCGRID_CELLTYPE_MIN + logodd_obs;

    tictac.Tic();
    for (i = 0; i < N; i++)
    {
      gridMap->updateCell_fast_occupied(
          2, 2, logodd_obs, logodd_thres_occupied, theMapArray, theMapSize_x);
    }
    double T = tictac.Tac();
    cout << "-> " << 1e9 * T / N << " ns/iter."
         << endl;  // the "p" is to avoid optimizing out the entire loop!
  }

#if 0
    for (i=50;i<51;i++)
    {
        CPose3D  pose3D(0.21,0.34,0,-2);
        //scan1.validRange.assign(scan1.validRange.size(), false);
        //scan1.getScanRangeValidity(i)=true;

        gridMap->clear();
        gridMap->resizeGrid(-5,20,-15,15);
        gridMap->insertObservation( &scan1, &pose3D );
        gridMap->saveAsBitmapFile(format("./gridmap_with_widening_%04i.png",i));
    }
#endif

  // test 5: Laser insertion
  // ----------------------------------------
  if (true)
  {
    gridMap->insertionOptions.wideningBeamsWithDistance = false;
    N = 3000;
    cout << "Running test #5: Laser insert. w/o widen... ";
    cout.flush();
    tictac.Tic();
    for (i = 0; i < N; i++)
    {
#if 1
      CPose2D pose(
          getRandomGenerator().drawUniform(-1.0, 1.0), getRandomGenerator().drawUniform(-1.0, 1.0),
          getRandomGenerator().drawUniform(-M_PI, M_PI));
      CPose3D pose3D(pose);
#else
      CPose3D pose3D;
#endif

      gridMap->insertObservation(scan1, pose3D);
    }
    double T = tictac.Tac();
    cout << "-> " << 1000 * T / N << " ms/iter, scans/sec:" << N / T << endl;

    CPose3D pose3D;
    gridMap->clear();
    gridMap->insertObservation(scan1, pose3D);
    gridMap->saveAsBitmapFile("./gridmap_without_widening.png");
  }

  // test 6: Laser insertion without widening
  // --------------------------------------------------
  if (true)
  {
    gridMap->insertionOptions.wideningBeamsWithDistance = true;
    N = 3000;
    cout << "Running test #6: Laser insert. widen... ";
    cout.flush();
    tictac.Tic();
    for (i = 0; i < N; i++)
    {
#if 1
      CPose2D pose(
          getRandomGenerator().drawUniform(-1.0, 1.0), getRandomGenerator().drawUniform(-1.0, 1.0),
          getRandomGenerator().drawUniform(-M_PI, M_PI));
      CPose3D pose3D(pose);
#else
      CPose3D pose3D;
#endif
      gridMap->insertObservation(scan1, pose3D);
    }
    double T = tictac.Tac();
    cout << "-> " << 1000 * T / N << " ms/iter, scans/sec:" << N / T << endl;

    CPose3D pose3D;
    gridMap->clear();
    gridMap->insertObservation(scan1, pose3D);
    gridMap->saveAsBitmapFile("./gridmap_with_widening.png");
  }

  // test 7: Grid resize
  // ----------------------------------------
  if (true)
  {
    N = 400;
    cout << "Running test #7: Grid resize... ";
    cout.flush();
    tictac.Tic();
    for (i = 0; i < N; i++)
    {
      *gridMap = gridMapCopy;
      gridMap->resizeGrid(-30, 30, -40, 40);
    }
    double T = tictac.Tac();
    cout << "-> " << 1000 * T / N << " ms/iter" << endl;
  }

  // test 8: Likelihood computation
  // ----------------------------------------
  if (true)
  {
    N = 5000;

    *gridMap = gridMapCopy;
    CPose3D pose3D(0, 0, 0);
    gridMap->insertObservation(scan1, pose3D);

    cout << "Running test #8: Likelihood... ";
    cout.flush();
    double R = 0;
    tictac.Tic();
    for (i = 0; i < N; i++)
    {
      const auto pose = mrpt::poses::CPose3D::FromXYZYawPitchRoll(
          getRandomGenerator().drawUniform(-1.0, 1.0), getRandomGenerator().drawUniform(-1.0, 1.0),
          .0, getRandomGenerator().drawUniform(-M_PI, M_PI), .0, .0);
      R += gridMap->computeObservationLikelihood(scan1, pose);
    }
    double T = tictac.Tac();
    cout << "-> " << 1000 * T / N << " ms/iter" << endl;
  }
}

int main(int argc, char** argv)
{
  try
  {
    // optional argument: a different ini file
    if (argc > 1) iniFile = string(argv[1]);

    BenchmarkGridmaps();
    return 0;
  }
  catch (exception& e)
  {
    cout << "MRPT exception caught: " << e.what() << endl;
    return -1;
  }
  catch (...)
  {
    printf("Another exception!!");
    return -1;
  }
}