Example: maps_gridmap_likelihood_characterization

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/COccupancyGridMap2D.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

#include <iostream>

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

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

// Default .ini file:
string iniFile(myDataDir + string("config_likelihood.ini"));

// ------------------------------------------------------
//              theMainThing
// ------------------------------------------------------
void theMainThing()
{
  COccupancyGridMap2D gridMap;
  CObservation2DRangeScan obsScan;
  CTicTac tictac;

  // Load the grid map from file
  // ----------------------------------------------
  obsScan.aperture = M_2PIf;

  ASSERT_(mrpt::system::fileExists(iniFile));
  mrpt::config::CConfigFile cfgFile(iniFile);

  string bmp = cfgFile.read_string("Params", "bitmap_file", "", true);
  float res = cfgFile.read_float("Params", "evaluation_grid_resolution", 0.1f, true);

  float evalgrid_x_min = 0, evalgrid_x_max = 0;
  float evalgrid_y_min = 0, evalgrid_y_max = 0;

  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(evalgrid_x_min, float, cfgFile, "Params");
  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(evalgrid_x_max, float, cfgFile, "Params");
  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(evalgrid_y_min, float, cfgFile, "Params");
  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(evalgrid_y_max, float, cfgFile, "Params");

  int type_experiment = 0;
  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(type_experiment, int, cfgFile, "Params");

  // Gridmap:
  string GRIDMAP_FILE(myDataDir + bmp);

  gridMap.loadFromBitmapFile(GRIDMAP_FILE, 0.05f);
  gridMap.saveAsBitmapFile("./out_gridmap.png");

  if (type_experiment == 0)
  {
    // Simulate scan:
    // ----------------------------------------------
    obsScan.aperture = M_PIf;
    obsScan.maxRange = 80.0f;
    obsScan.rightToLeft = true;
    gridMap.laserScanSimulator(obsScan, CPose2D(0, 0, 0), 0.5f, 180);
  }
  else
  {
    // Lik. of each 2D  position
    // ------------------------------------
    obsScan.aperture = 0;
    obsScan.maxRange = 80.0f;
    obsScan.rightToLeft = true;
    obsScan.resizeScanAndAssign(1, 0.0, true);
  }

  // Set options:
  // ----------------------------------------------
  gridMap.likelihoodOptions.loadFromConfigFile(cfgFile, "LikelihoodOptions");
  gridMap.likelihoodOptions.dumpToConsole();

  // Perform simulation:
  // ----------------------------------------------
  printf("Performing simulation (saving to out_lik.txt)...");

  FILE* f = os::fopen("out_lik.txt", "wt");
  ASSERT_(f);

  CSimplePointsMap pointsMap;
  pointsMap.clear();
  pointsMap.insertPoint(0, 0);

  tictac.Tic();

  float phi = (float)0.0_deg;

  auto nullPose = CPose3D(0, 0, 0, phi, 0, 0);

  for (float y = evalgrid_y_min; y < evalgrid_y_max; y += res)
  {
    for (float x = evalgrid_x_min; x < evalgrid_x_max; x += res)
    {
      nullPose.x(x);
      nullPose.y(y);
      fprintf(f, "%e ", gridMap.computeObservationLikelihood(obsScan, nullPose));
    }  // for y
    fprintf(f, "\n");
  }  // for x

  printf("Done!\n");

  printf("Time:%fms\n", 1000.0f * tictac.Tac());

  os::fclose(f);
}

int main()
{
  try
  {
    theMainThing();

    return 0;
  }
  catch (const std::exception& e)
  {
    std::cout << "MRPT exception:\n" << e.what() << std::endl;
    return -1;
  }
  catch (...)
  {
    printf("Runtime exception!!");
    return -1;
  }
}