Home » 2014 (Page 2)

# Yearly Archives: 2014

## Voronoi Road Map, Path Planing

clc;close all;clear all;

Input Map

```RGB = imread('Map.png');
figure('Position',[100 0 600 700],'color','k');
imagesc(RGB);
axis image
title('Input Map','color','w');
axis off
set(gcf, 'InvertHardCopy', 'off');
hold off;```

Convert to binary image matrix and inverse matrix values

```I = rgb2gray(RGB);
binaryImage = im2bw(RGB, 0.3);
sz  = size(binaryImage);```

Inital pose

```xs = 225; ys = 355;
% Goal Pose
xg = 50; yg = 50;
% Pose Matrix
pose = zeros(sz(1),sz(2));
pose(ys,xs) = 1;
pose(yg,xg) = 1;```

```figure('Position',[100 0 600 700],'color','k');
hold on;
set(gcf, 'InvertHardCopy', 'off');
sk = 1;
for i = 1:sz(1)
for j = 1:sz(2)
if(binaryImage(i,j) == 1 )
xv(sk) = j;
yv(sk) = i;
sk = sk+1;
end
end
end
[vrx,vry]  = voronoi(xv,yv);
set(gca,'YDir','Reverse');
plot(xv,yv,'y.',vrx,vry,'b-');
axis tight
%axis([1 sz(2) 1 sz(1) ]) ;
axis image
'* - Represents Starting and Goal Position '},'color','w');
axis off
spy(pose,'*r');```

Robot Path

```x = xs; y = ys;
indk = 1;
while (abs(x - xg) > 5)||(abs(y - yg)> 5)
path(:,indk) = [x y];
dist = sqrt((vrx - x).^2+(vry - y).^2);
goal = sqrt((xg - x).^2+(xg - y).^2);
% Find closest vertices of the Voronoi edges
[mn ind] = min(dist(:));
xt = vrx(ind);
yt = vry(ind);
goalj = sqrt((xg - xt).^2+(xg - yt).^2);
if (goalj < goal )
x = xt;
y = yt;
vrx(ind) = 1E6;
vry(ind) = 1E6;
indk = indk +1;
else
vrx(ind) = 1E6;
vry(ind) = 1E6;
end
end
path(:,indk) = [xg yg];
plot(path(1,:),path(2,:),'-r');
title({'Voronoi Road Map and Robot Path';
'* - Represents Starting and Goal Position '},'color','w');```

## Potential Field Path Planning

clc;close all;clear all;

Input Map

```RGB = imread('Mapn.png');
figure('Position',[600 0 600 1000],'color','k');
image(RGB);
hold on;
axis off
axis image```

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;```

Attractive potential

```sz  = size(binaryImage);
for i = 1:sz(1)
for j = 1:sz(2)
rg = sqrt((i-yg)^2 +(j - xg)^2);
U_att(i,j) = 0.5*rg^2;
end
end
contour(U_att,40);

set(gcf, 'InvertHardCopy', 'off');
% Pose Matrix
pose = zeros(sz(1),sz(2));
pose(ys,xs) = 1;
pose(yg,xg) = 1;
spy(pose,'*r');
title({'Attractive Potential Field';...
'Circles Center Corespondents to the Goal Position '},'color','w');
hold off;```

Repulsive potential

```Di = 35; % Distance of influence of the object
% 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)
U_rep(i,j)  = 0.5*(rd - Di)^2;
else
U_rep(i,j)  = 0;
end
end
end
figure('Position',[600 0 600 1000],'color','k');
hold on;
colormap jet;
contourf(U_rep,5);
spy(pose,'*r');
axis off
axis image
set(gcf, 'InvertHardCopy', 'off');
title('Repulsive Potential Field','color','w');
hold off;```

Summary Potential Field

```k_s  = 2; % Scaling factor
U_sum = U_rep/max(U_rep(:))+k_s*U_att/max(U_att(:));
figure('Position',[600 0 600 1000],'color','k');
hold on;
contourf(U_sum,15);
spy(pose,'*r');
axis off
axis image
title('Summary Potential Field','color','w');
set(gcf, 'InvertHardCopy', 'off');
hold off;```

Potential Field Path Planning

```figure('Position',[600 0 600 1000],'color','k');
hold on;
contourf(U_sum,15);
spy(pose,'*r');
axis off
axis image
title('Potential Field Path Planning,*Note the Potential Trap','color','w');
x = xs; y = ys;
last = U_sum(y-1,x-1);
while (x ~= xg)||(y ~= yg)
dis =[ U_sum(y-1,x-1), U_sum(y-1,x),U_sum(y-1,x+1);
U_sum(y,x-1), U_sum(y,x) ,U_sum(y,x+1);
U_sum(y+1,x-1), U_sum(y+1,x),U_sum(y+1,x+1)];
m = min(dis(:));
[r,c] = find(dis == m);
U_sum(y,x) = inf;
y = y-2+r;
x = x-2+c;
pose(y,x) = 1;
end
spy(pose,'.r');
set(gcf, 'InvertHardCopy', 'off');
hold off;```

## Computing a Skeleton of a Map for Mobile Robots,Path Planning

clc;close all;clear all;

Input Map

```RGB = imread('Map1.png');
figure('Position',[0 0 800 800]);
imshow(RGB);```

Convert to binary image matrix and inverse matrix values

```I = rgb2gray(RGB);
binaryImage = im2bw(RGB, 0.3);
binaryImage = 1-binaryImage;
figure('Position',[0 0 800 800]);
imshow(binaryImage);```
```Warning: Image is too big to fit on screen; displaying at
50%```

Compute for every pixel the distance to the nearest obstacle(non zero elements which after inversion corespondent to the obstacles)

```[D,IDX] = bwdist(binaryImage);
figure('Position',[0 0 800 800],'color','k')
colormap bone
image(D);
axis off          % Remove axis ticks and numbers
axis image
set(gcf, 'InvertHardCopy', 'off');```

Taking the Laplacian of the distance transform. Play with the treshold value 0.15 to remove or add more futures in skeleton

```lap = del2(D);
sz  = size(lap);
for i = 1:sz(1)
for j = 1:sz(2)
if( abs(lap(i,j)) < 0.15)
lap(i,j) = 0;
end
end
end
lap = flipdim(lap ,1);           %# vertical flip
figure('Position',[0 0 800 800],'color','k');
colormap winter;
contour(lap,'DisplayName','lap');
axis off
axis image
set(gcf, 'InvertHardCopy', 'off');```

## Swirl,Matlab Code

```clc; clear all; close all;
x = -20:0.1:20;
sz = size(x);
ind = 0;
b = 6
for n = 1:15 %
for i = 1:sz(2)
for j = 1:sz(2)
sw(i,j) = sin(b*cos(sqrt(x(i)^2+x(j)^2))-n*atan2(x(i),x(j)));
end
end
ind = ind +1;
fig = figure('Position',[0 0 800 800]);
hold on;
imagesc(sw);
colormap bone;
axis off;
set(fig, 'color', [0 0 0]);
set(gcf, 'InvertHardCopy', 'off');
hold off;
%print(fig,['Sw',num2str(ind)],'-djpeg  ','-r300');
close all;
end```

## Ulam’s Prime Spiral

To generate a Ulam’s prime spiral the positive integers are arranged in a spiral pathern and prime numbers are highlighted in some way along the spiral. For example we used blue pixels to represent primes and white pixels for composite numbers. The code below uses built in Matlab function to generate the Ulam spiral.

```close all;
clc
clear all
sz  = 101; % Size of the NxN square matrix
mat = spiral(sz);
k =1;
for i =1:sz
for j=1:sz
if isprime(mat(i,j)) % Check if the number is prime
% saving indices of primes
y(k) = i;
x(k) = j;
k = k+1;
end
end
end
figure('Position',[0 0 800 800]);
hold on;
colormap bone;
scatter(x,y,'.b');
axis([1 sz  1 sz ]);
axis off;
set(gca,'YDir','Reverse');
```

Now lets construct Varanoi diagram for the prime numbers (higlighted points in blue) in Ulam spiral.

```figure('Position',[0 0 800 800]);
hold on;
xy = [x',y'];
[v,c] = voronoin(xy);
for i = 1:length(c)
patch(v(c{i},1),v(c{i},2),'w');
end
axis([1 sz  1 sz ]); axis off;
set(gca,'YDir','Reverse');
scatter(x,y,'.b');
```

## Cells in Voronoi diagram of the Ulam Prime Spiral

```% We observe various poligons with  n-sides such as triangles, quadrangles, pentagons, etc...
% Lets call
% A3 = a sequence of prime numbers which has a Triangular Voronoi cell in Voronoi diagram of the Ulam Prime Spiral
% A4 = a sequence of prime numbers which has a Quadrilateral Voronoi cell in Voronoi diagram of the Ulam Prime Spiral
% A5 = a sequence of prime numbers which has a Pentagonal Voronoi cell in Voronoi diagram of the Ulam Prime Spiral
% A6 = 6 sides
% A7 = 7 sides
% An = n sides
% Q1: What is the maximum value for n, n_max ? In other words is there an uper bound ?
% Q2: If yes, What are the primes whic has a n_max side poligon cells in Voronoi diagram of the Ulam Prime Spiral
% The code below is used to calculate An
k3 = 1; k4 = 1; k5 = 1; k6 = 1; k7 = 1; k8 = 1;
for i = 1:length(c)
szv = size(v(c{i},1));
polyN(i) = szv(1);
switch polyN(i)
case 3
A3(k3) = mat(y(i),x(i));
k3 = k3+1;
A3xy(k3,:)= [x(i),y(i)];
case 4
A4(k4) = mat(y(i),x(i));
k4 = k4+1;
A4xy(k4,:)= [x(i),y(i)];
case 5
A5(k5) = mat(y(i),x(i));
k5 = k5+1;
A5xy(k5,:)= [x(i),y(i)];
case 6
A6(k6) = mat(y(i),x(i));
k6 = k6+1;
A6xy(k6,:)= [x(i),y(i)];
case 7
A7(k7) = mat(y(i),x(i));
k7 = k7+1;
A7xy(k7,:)= [x(i),y(i)];
case 8
A8(k8) = mat(y(i),x(i));
k8 = k8+1;
A8xy(k8,:)= [x(i),y(i)];
end
end
%
% First 15 terms
A3 = sort(A3);
fprintf('A3 = ');
fprintf('%i, ',A3(1:15));
fprintf('\n');
A4 = sort(A4);
fprintf('A4 = ');
fprintf('%i, ',A4(1:15));
fprintf('\n');
A5 = sort(A5);
fprintf('A5 = ');
fprintf('%i, ',A5(1:15));
fprintf('\n');
A6 = sort(A6);
fprintf('A6 = ');
fprintf('%i, ',A6(1:15));
fprintf('\n');
A7 = sort(A7);
fprintf('A7 = ');
fprintf('%i, ',A7(1:15));
fprintf('\n');
A8 = sort(A8);
fprintf('A8 = ');
fprintf('%i, ',A8(1:15));
fprintf('\n');
```
```A3 = 313, 389, 1283, 1399, 1669, 1787, 2087, 2143, 2713, 2801, 3469, 4091, 4787, 4789, 4903,
A4 = 23, 31, 47, 59, 71, 73, 79, 131, 139, 167, 173, 181, 229, 239, 251,
A5 = 2, 3, 11, 13, 17, 19, 29, 37, 53, 83, 97, 101, 103, 107, 109,
A6 = 5, 7, 41, 43, 89, 127, 179, 193, 233, 263, 283, 317, 379, 383, 397,
A7 = 61, 157, 199, 311, 349, 409, 463, 509, 557, 601, 641, 691, 727, 757, 823,
A8 = 67, 491, 613, 1013, 1117, 1201, 1249, 1301, 1373, 1543, 1753, 1907, 2017, 2339, 2411,
```
```% Plotting
% The color code used
% n = 3, Triangle yellow
% n = 4, Tetragon green
% n = 5, Pentagon magenta
% n = 6, cyan
% n = 7, red
% n = 8, yellow

figure('Position',[0 0 800 800]);
hold on;
xy = [x',y'];
[v,c] = voronoin(xy);
for i = 1:length(c)
patch(v(c{i},1),v(c{i},2),'w');
end
axis([1 sz  1 sz ]); axis off;
set(gca,'YDir','Reverse');
scatter(A3xy(:,1),A3xy(:,2),'.k');
scatter(A4xy(:,1),A4xy(:,2),'.g');
scatter(A5xy(:,1),A5xy(:,2),'.m');
scatter(A6xy(:,1),A6xy(:,2),'.c');
scatter(A7xy(:,1),A7xy(:,2),'.r');
scatter(A8xy(:,1),A8xy(:,2),'.y');
% Note that the last terms can be wrong. They corespond to the points on the outer
% edges of the spiral which might be altered when considering a larger spiral.
```

## Triangular Cells with Integer Area

```k3 = 1;
k3i = 1;
fprintf('Prime, Cell Area, Perimeter \n');
for i = 1:length(c)
szv = size(v(c{i},1));
polyN(i) = szv(1);
if(polyN(i) == 3)
% Sides of a triangle
A = v(c{i}(1, 1),:) - v(c{i}(1, 2),:);
B = v(c{i}(1, 1),:) - v(c{i}(1, 3),:);
C = v(c{i}(1, 2),:) - v(c{i}(1, 3),:);
% Perimeter of a triangle
pv = norm(A)+ norm(B)+ norm(C);
P3(k3) = pv;
% Area of a triangle
AB = cross([A,0], [B,0], 2);
S3(k3) =  1/2 * sum(sqrt(sum(AB.^2, 2)));
A3(k3) = mat(y(i),x(i));
fprintf('%i %4.4f %4.4f \n',A3(k3), S3(k3),P3(k3))
%  Primes with triangle Voronoi cells with integer area
if S3(k3)== round(S3(k3)) % check if area is integer
A3i(k3i) = mat(y(i),x(i));
k3i = k3i +1;
end
k3 = k3+1;
end
end
% Primes with triangular Voronoi cells with integer area
fprintf('\n A3i = ');
A3i = sort(A3i);
fprintf('%i, ',A3i);
```
```Prime, Cell Area, Perimeter
8999 9.0000 14.4853
9001 9.0000 14.4853
8627 4.5000 10.2426
10093  NaN  Inf
6869 8.0000 13.6569
9419 6.0000 11.4049
5867 6.0000 11.4049
3469 6.0000 11.4049
2801 6.0000 11.4049
1669 4.5000 10.2426
9439 4.5000 10.2426
4787 6.0000 11.4049
4789 6.0000 11.4049
2143 4.5000 10.2426
1787 4.5000 10.2426
4933 6.0000 11.4049
313 6.0000 11.4049
389 6.0000 11.4049
1399 6.0000 11.4049
2713 4.5000 10.2426
1283 6.0000 11.4049
2087 4.0000 9.6569
4091 6.0000 11.4049
4903 6.2500 12.0711
8111 6.0000 11.4049
6037 6.2500 12.0711
10007  NaN  Inf
9929  NaN  Inf

A3i = 313, 389, 1283, 1399, 2087, 2801, 3469, 4091, 4787, 4789, 4933, 5867, 6869, 8111, 8999, 9001, 9419,```