Function createGaussianWorld

grid_map_core/test/test_helpers.cpp:124–167

}

AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map)
{

  struct Gaussian
  {
    double x0, y0;
    double varX, varY;
    double s;
  };

  AnalyticalFunctions func;

  std::uniform_real_distribution<double> var(0.1, 3.0);
  std::uniform_real_distribution<double> mean(-4.0, 4.0);
  std::uniform_real_distribution<double> scale(-3.0, 3.0);
  constexpr int numGaussians = 3;
  std::array<Gaussian, numGaussians> g;

  for (int i = 0; i < numGaussians; ++i) {
    g.at(i).x0 = mean(rndGenerator);
    g.at(i).y0 = mean(rndGenerator);
    g.at(i).varX = var(rndGenerator);
    g.at(i).varY = var(rndGenerator);
    g.at(i).s = scale(rndGenerator);
  }

  func.f_ = [g](double x,double y) {
    double value = 0.0;
    for (int i = 0; i < g.size(); ++i) {
      const double x0 = g.at(i).x0;
      const double y0 = g.at(i).y0;
      const double varX = g.at(i).varX;
      const double varY = g.at(i).varY;
      const double s = g.at(i).s;
        value += s * std::exp(-(x-x0)*(x-x0) / (2.0*varX) - (y-y0)*(y-y0) / (2.0 * varY));
    }

    return value;
  };

  fillGridMap(map, func);

  return func;
}

void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions)
{