Function createGaussianWorld

grid_map_demos/src/InterpolationDemo.cpp:111–156

}

AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map)
{

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

  AnalyticalFunctions func;
  constexpr int numGaussians = 7;
  std::array<std::pair<double, double>, numGaussians> vars = { { { 1.0, 0.3 }, { 0.25, 0.25 }, {
      0.1, 0.1 }, { 0.1, 0.1 }, { 0.1, 0.1 }, { 0.1, 0.05 }, { 0.05, 0.05 } } };
  std::array<std::pair<double, double>, numGaussians> means = { { { 1, -1 }, { 1, 1.7 },
      { -1, 1.6 }, { -1.8, -1.8 }, { -1, 1.8 }, { 0, 0 }, { -1.2, 0 } } };
  std::array<double, numGaussians> scales = { -2.0, -1.0, 2.0, 1.0, 3.0, 4.0, 1.0 };

  std::array<Gaussian, numGaussians> g;

  for (int i = 0; i < numGaussians; ++i) {
    g.at(i).x0 = means.at(i).first;
    g.at(i).y0 = means.at(i).second;
    g.at(i).varX = vars.at(i).first;
    g.at(i).varY = vars.at(i).second;
    g.at(i).s = scales.at(i);
  }

  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)
{