Filtering point cloud for local costmap