Add bound checks for bilinear interpolation

[aalbaali]

Issue

Linear interpolation results in wrong values when queried position is near the map bounds.

I encountered this problem when trying to interpolate a value in a grid map of distance fields, where all values are positive. The result was negative, which didn't align with my expectations. Upon some digging, I noticed that the linear interpolater doesn't check if the intermediate positions are out of bounds, which result in strange behaviour.

Solution

Check that the interpolation points are within the grid map bounds.

Minimum working example

#include <iostream>
#include "grid_map_core/grid_map_core.hpp"

int main() {
  grid_map::GridMap map{};
  map.setGeometry({3, 3}, 1.0);
  const std::string layer = "distance";
  map.add(layer, 0.0);
  // clang-format off
  map[layer] << 
    0.0, 0.0, 0.0,
    0.0, 0.0, 1.0,
    0.0, 1.0, 3.0;
  // clang-format on

  const grid_map::Position pos{-1.49, -1.0};
  if (!map.isInside(pos)) {
    std::cerr << "Position outside map" << std::endl;
    return 1;
  }
  const auto val = map.atPosition(layer, pos, grid_map::InterpolationMethods::INTER_LINEAR);
  std::cout << "Val: " << val << std::endl;
}

Outputs before the fix:

Val: -1.47

which is a negative value even though all values in the map are non-negative.

Output after the fix (it defers to nearest-neighbour):

Val: 3