Implementation of "Distance Transforms of Sampled Functions"