Function firstOrderDerivativeAt

grid_map_core/src/CubicInterpolation.cpp:346–374

}

double firstOrderDerivativeAt(const Matrix &layerData, const Index &index, Dim2D dim,
                              double resolution)
{
  const int numCol = layerData.cols();
  const int numRow = layerData.rows();

  double left, right;
  switch (dim) {
    case Dim2D::X: {
      left = layerData(bindIndexToRange(index.x() + 1, numRow), index.y());
      right = layerData(bindIndexToRange(index.x() - 1, numRow), index.y());
      break;
    }
    case Dim2D::Y: {
      left = layerData(index.x(), bindIndexToRange(index.y() + 1, numCol));
      right = layerData(index.x(), bindIndexToRange(index.y() - 1, numCol));
      break;
    }
    default: {
      throw std::runtime_error("Unknown derivative direction");
    }
  }

  const double perturbation = resolution;
  // central difference approximation
  // we need to multiply with resolution since we are
  // operating in scaled coordinates
  return (right - left) / (2.0 * perturbation) * resolution;
}

double mixedSecondOrderDerivativeAt(const Matrix &layerData, const Index &index, double resolution)
{