Function computeInterpolationError

grid_map_demos/src/InterpolationDemo.cpp:206–240

}

Error computeInterpolationError(const AnalyticalFunctions &groundTruth,
                                const grid_map::GridMap &map)
{
  const double r = map.getResolution();
  const double dimX = map.getLength().x() / 2.0 - 3.0 * r;
  const double dimY = map.getLength().y() / 2.0 - 3.0 * r;

  unsigned int count = 0;
  Error error;
  const int nRow = map.getSize().x();
  const int nCol = map.getSize().y();
  for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
    const auto row = (*iterator).x();
    const auto col = (*iterator).y();
    const bool skipEvaluation = row < 2 || col < 2 || col > (nCol - 3) || row > (nRow - 3);
    if (skipEvaluation) {
      continue;
    }
    grid_map::Position pos;
    map.getPosition(*iterator, pos);
    const double f = map.at(demoLayer, *iterator);
    const double f_ = groundTruth.f_(pos.x(), pos.y());
    const double e = std::fabs(f - f_);
    error.meanSquare_ += e * e;
    error.meanAbs_ += e;
    ++count;
    if (e > error.max_) {
      error.max_ = e;
    }
  }
  error.meanSquare_ /= count;
  error.meanAbs_ /= count;
  return error;

}

InterpolationDemo::InterpolationDemo(ros::NodeHandle *nh)
{