Full code

%% Load image

close all; clear all; clc

camera1 =imread('http://172.16.6.1/axis-cgi/jpg/image.cgi?resolution=800X600');

camera2 =imread('http://172.16.6.2/axis-cgi/jpg/image.cgi?resolution=800X600');

camera3 =imread('http://172.16.6.3/axis-cgi/jpg/image.cgi?resolution=800X600');

camera4 = zeros(size(camera1));

camera5 = zeros(size(camera1));

camera6 = zeros(size(camera1));

%camera6 = camera6(1:560,1:90,1:end);

camera3 = camera3(1:560,1:472,1:end);

camera4 = camera4(110:end,1:455,1:end);

camera1 = camera1(1:560,18:end,1:end);

%camera5 = camera5(1:560,661:end,1:end);

camera2 = camera2(110:end,1:end,1:end);

camera_full = [camera3,camera1;camera4,camera2];%

camera_full=imresize(camera_full,.5);

%rgb_img = imread('http://172.16.6.1/axis-cgi/jpg/image.cgi?resolution=800X600');

rgb_img = camera_full;

% rgb_img = imread('fudicial_arrow.jpg');

cform = makecform('srgb2lab'); % Make a converter for L+ab color space

lab_img = applycform(rgb_img,cform);  % Apply the conversion. Now ready!

% The color information is in dimensions 2 and 3.

ab = double(lab_img(:,:,2:3));

nrows = size(ab,1);

ncols = size(ab,2);

ab = reshape(ab,nrows*ncols,2);

%% POLAR projection AND keep data only far away from center

Xcenter = 127;

Ycenter = 127;

radius = 17; % ignore everything in this radius

% Shift the x and y axes of data

ab_shifted = [ab(:,1)-Xcenter, ab(:,2)-Ycenter];

[theta_ab,R_ab] = cart2pol(ab_shifted(:,1),ab_shifted(:,2));

theta_ab = theta_ab*180/pi;

% Block out the center region

bad_indices = find(R_ab<radius);

theta_ab(bad_indices) = 195;

R_ab(bad_indices) = 0;

%% Grab only RED pixels

%I = double(rgb_img(:,:,1))+ double(rgb_img(:,:,2))+ double(rgb_img(:,:,3));

% thresh_I = (I/max(max(I)))>0.93;

empty_intensity = 0*rgb2gray(rgb_img); % create an empty intensity matrix

red_indices = find(0<theta_ab & theta_ab<30); % threshold on red

empty_intensity(red_indices) = 255; % operate on matrix like column vector

% green_indices = find(130<theta_ab & theta_ab<180);

% empty_intensity(green_indices) = 255;

% blue_indices=find(-180<theta_ab & theta_ab<-90);

% empty_intensity(blue_indices)=255;

bw = im2bw(empty_intensity,0.5); % threshold image to produce pure black/white

% bw=bw+thresh_I;

bw(182:400,510:end) = 1;

bw(238:331,1:300) = 1;

%imshow(bw); % show result

%% Remove noise... not necessary for this image, but might be later

% remove all object containing fewer than 30 pixels

%bw = bwareaopen(bw,15,8);

%imshow(bw)

% se = strel('disk',2);

% bw = imclose(bw,se);

% %imshow(bw)

% bw2=fcn_dilate_by_n(bw,8);

% imshow(bw2)

% filled=regionprops(bw2,'Centroid','Perimeter');

% perm=cat(1,filled.Perimeter);

% cent=cat(1,filled.Centroid);

% indices_destroy=find(perm<200);

% for i=1:length(indices_destroy)

%    c(i)=cent(indices_destroy(i),1);

%        r(i)=cent(indices_destroy(i),2);

% end

% BW2 = bwselect(bw2,c,r,4);

% bw2=bw2-BW2;

% imshow(bw2)

% % % fill a gap in the pen's cap

% % B=bwperim(bw2);

% %  imshow(B)

% %

% % bw2 = bwareaopen(bw2,900);

% % imshow(bw2)

% % fill any holes, so that regionprops can be used to estimate

% % the area enclosed by each of the boundaries

% %bw = imfill(bw,'holes');

%  bw2=fcn_dilate_by_n(bw2,10);

%imshow(bw2)

%cc = bwconncomp(bw);

%%

bw = bwareaopen(bw,15,8);

% figure

% imshow(bw)

se = strel('disk',1);

bw2 = imclose(bw,se);

%imshow(bw)

% figure

bw2=fcn_dilate_by_n(bw2,8);

%imshow(bw2)

filled=regionprops(bw2,'Centroid','Area','Perimeter');

perm=cat(1,filled.Perimeter);

cent=cat(1,filled.Centroid);

%a = cat(1,filled.Area);

indices_destroy=find(perm<100);

%indices_destroy=find(a<500);

for i=1:length(indices_destroy)

    c(i)=cent(indices_destroy(i),1);

    r(i)=cent(indices_destroy(i),2);

end

try

    BW2 = bwselect(bw2,c,r,4);

catch

    BW2 = 0;

end

bw2=bw2-BW2;

bw2=fcn_dilate_by_n(bw2,8);

map=(bw2);

r = input('Which exit will be exiting from today, Gentlemen?');

if r == 1

    %startPoint=[165,376];

    goalPoint=[72,620];

    disp('Oh very good, sir')

    %map = imrotate(map,pi/2);

end

if r == 2

    %    startPoint=[521,564];

    goalPoint=[526,570];

    disp('Oh very good, sir')

    %map = imrotate(map,);

end

if r == 3

    %    startPoint=[206,500];

    goalPoint=[73,21];

    disp('Oh very good, sir')

end

startPoint2 = round(Path_find_yellow(rgb_img));

startPoint = round(mean(startPoint2));

startPoint = fliplr(startPoint);

%startPoint = [156,395];

times = [];

path=[];

showSteps=true;

%mapWait=load('mapWait.txt');

colormap([1 1 1; 0 0 0;1 0 0]);

[m,n] = size(map);

quadrantPos = 0;

disp('Starting Team 2 Potential Dfield method');

[path,distance]=path_CalculatePathMethodD2Star(m,n,map,startPoint,quadrantPos,goalPoint)

% Plot result

mapPlot=map;

for j=1:length(path)

    mapPlot(path(j,1),path(j,2))=2;

end

image(mapPlot+1);

title(sprintf('Team 2 Potential D-field METHOD - press any key to continue - map # %d/%d'));

pause(0.1);

hold on

%% Corner

x=mapPlot-map;

C=corner(x,'MinimumEigenvalue');

C = rot90(C,2);

theta = fcn_angle(C(:,1),startPoint2)

%% Find boundaries so that we can get region properties

% [B,L] = bwboundaries(bw2,'noholes');

% % Display the label matrix and draw each boundary

% imshow(label2rgb(L, @jet, [.5 .5 .5]))

% hold on

% for k = 1:length(B)

%   boundary = B{k};

%   plot(boundary(:,2), boundary(:,1), 'w', 'LineWidth', 2)

% end

%% Find the centroid position

% s  = regionprops(bw, 'Centroid','Orientation','MajorAxisLength','Area','MinorAxisLength');

% centroids = cat(1, s.Centroid);

% major = cat(1,s.MajorAxisLength);

% Orientation = cat(1,s.Orientation);

% area=cat(1,s.Area);

% minor=cat(1,s.MinorAxisLength);

% % Find our position by our dimensions

% inda=find(area>500 & area<600);

% indmaj=find(major>45 & major<55);

% indmin=find(minor>15 & minor<25);

% % Find our indices to get our centroid position

% if length(inda)==1

%     bingo=inda;

% elseif length(indmaj)==1

%         bingo=indmaj;

% elseif length(indmin)==1;

%             bingo=indmin;

%        else

%             bingo=1;

%         end

%imshow(bw) % Uncomment to show b/w image

% imshow(rgb_img);  % Uncomment to show original image

% hold on

% plot(centroids(:,1), centroids(:,2), 'b*');

% X1 = [centroids(:,1)];

% Y1 = [centroids(:,2)];

% X2 = [centroids(:,1)+MajorAxisLength(:,1).*cos(Orientation(:,1)*pi/180)];

% Y2 = [centroids(:,2)+MajorAxisLength(:,1).*sin(Orientation(:,1)*pi/180)];

% plot_arrow(X1,Y1,X2,Y2,'r','LineWidth',4); % Download this function from MATLAB Central

% hold off

%% To check for round objects...

% See http://www.mathworks.com/products/image/examples.html?file=/products/demos/shipping/images/ipexroundness.html

% stats = regionprops(L,'Area','Centroid','Perimeter');

% %per=cat(1,stats.Perimeter);

% threshold = 0.80;

% % loop over the boundaries

% for k = 1:length(B)

%   % obtain (X,Y) boundary coordinates corresponding to label 'k'

%   boundary = B{k};

%   % compute a simple estimate of the object's perimeter

%   delta_sq = diff(boundary).^2;

%   perimeter = sum(sqrt(sum(delta_sq,2)));

%   % obtain the area calculation corresponding to label 'k'

%   area = stats(k).Area;

%   % compute the roundness metric

%   metric = 4*pi*area/perimeter^2;

%   % display the results

%   metric_string = sprintf('%2.2f',metric);

%   % mark objects above the threshold with a black circle

%   if metric > threshold

%     centroid = stats(k).Centroid;

%     plot(centroid(1),centroid(2),'ko');

%   end

%   text(boundary(1,2)-35,boundary(1,1)+13,metric_string,'Color','y',...

%        'FontSize',14,'FontWeight','bold');

% per(k)=stats(k).Perimeter;

% end

% title(['Metrics closer to 1 indicate that ',...

%        'the object is approximately round']);

%% To check for nearby colors, add the bw images (logical add) and spread

%http://blogs.mathworks.com/steve/2010/09/07/almost-connected-component-labeling/