Input Map

RGB = imread('Mapn.png');
figure('Position',[600 0 600 1000],'color','k');
image(RGB);
hold on;
axis off
axis image
hold off;
% Convert to binary image matrix and inverse matrix values
I = rgb2gray(RGB);
binaryImage = im2bw(RGB, 0.3);
binaryImage = 1-binaryImage;
% Inital pose
xs = 540; ys = 750;
% Goal Pose
xg = 150; yg = 150;

Distance to Goal

sz  = size(binaryImage);
for i = 1:sz(1)
    for j = 1:sz(2)
       rg(i,j) = sqrt((i-yg)^2 +(j - xg)^2);
      end
end
% Pose Matrix
pose = zeros(sz(1),sz(2));
pose(ys,xs) = 1;
pose(yg,xg) = 1;

Di = 15; % Distance of influence of the obstacles
% Compute for every pixel the distance to the nearest obstacle(non zero
% elements which after inversion corespondent to the obstacles)
[D,IDX] = bwdist(binaryImage);
for i = 1:sz(1)
    for j = 1:sz(2)
        rd = D(i,j);
        if (rd <= Di)
            rg(i,j)  = 1e6;
        end
    end
end

Distance Transform Path Planning Algorithm

figure('Position',[600 0 600 1000],'color','k');
hold on;
image(RGB);
colormap gray
contour(rg,15);
spy(pose,'*r');
axis off
axis image
title('Distance Transform Path Planning Algorithm','color','w');
x = xs; y = ys;
last = rg(y-1,x-1);
while (x ~= xg)||(y ~= yg)
    dis =[ rg(y-1,x-1), rg(y-1,x),rg(y-1,x+1);
        rg(y,x-1), rg(y,x) ,rg(y,x+1);
        rg(y+1,x-1), rg(y+1,x),rg(y+1,x+1)];
     m = min(dis(:));
     [r,c] = find(dis == m);
   rg(y,x) = 1E6;
   y = y-2+r;
   x = x-2+c;
   pose(y,x) = 1;
  end
spy(pose,'.r');
set(gcf, 'InvertHardCopy', 'off');
hold off;