Add bwdist Function in C++

[mattking-smith]

Issue Purpose

Currently the bwdist.m is implemented in the MATLAB toolbox for computing the distance transform of binary image, which is needed to for determining the sign distance fields in obstacle factors. Unfortunately that particular MATLAB function needs the image processing toolbox in MATLAB.

Purposed resolution

Write a C++ equivalent of bwdist.m such that we remove the image processing toolbox dependency in MATLAB GPMP2 implementation.