Function bindIndicesToRange

grid_map_core/src/CubicInterpolation.cpp:299–332

}

void bindIndicesToRange(const GridMap &gridMap, IndicesMatrix *indices)
{
  const int numCol = gridMap.getSize().y();
  const int numRow = gridMap.getSize().x();

  //top left
  {
    const unsigned int iBoundToRange = bindIndexToRange(indices->topLeft_.x(), numRow);
    const unsigned int jBoundToRange = bindIndexToRange(indices->topLeft_.y(), numCol);
    indices->topLeft_ = Index(iBoundToRange, jBoundToRange);
  }

  //top right
  {
    const unsigned int iBoundToRange = bindIndexToRange(indices->topRight_.x(), numRow);
    const unsigned int jBoundToRange = bindIndexToRange(indices->topRight_.y(), numCol);
    indices->topRight_ = Index(iBoundToRange, jBoundToRange);
  }

  //bottom left
  {
    const unsigned int iBoundToRange = bindIndexToRange(indices->bottomLeft_.x(), numRow);
    const unsigned int jBoundToRange = bindIndexToRange(indices->bottomLeft_.y(), numCol);
    indices->bottomLeft_ = Index(iBoundToRange, jBoundToRange);
  }

  //bottom right
  {
    const unsigned int iBoundToRange = bindIndexToRange(indices->bottomRight_.x(), numRow);
    const unsigned int jBoundToRange = bindIndexToRange(indices->bottomRight_.y(), numCol);
    indices->bottomRight_ = Index(iBoundToRange, jBoundToRange);
  }

}

bool getFirstOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices, Dim2D dim,
                              double resolution, DataMatrix *derivatives)