Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add bound checks for bilinear interpolation #459

Open
wants to merge 1 commit into
base: master
Choose a base branch
from

Conversation

aalbaali
Copy link

@aalbaali aalbaali commented May 26, 2024

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

- Fixes issues of getting negative interpolated value when all values in matrix are positive

Signed-off-by: aalbaali <[email protected]>
@Ryanf55 Ryanf55 added ros1 Affects ROS 1 bug labels Jul 19, 2024
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
bug ros1 Affects ROS 1
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants