I have the following code to generate a PRM map that will be used for A* application. There exist 2 problems with the code
- It keeps the blue lines representing the original PRM where lines can cross over obstacles. I don't want to keep the blue lines but I couldn't find the way to remove them.
- The green lines are going over obstacles even though they shouldn't
The code is as follows
clc;
clear all;
close all;
seed = 123512;
rng(seed);
xaxis = 100;
yaxis = 100;
obstacles = false(xaxis,yaxis);
[X,Y] = meshgrid(1:xaxis,1:yaxis);
obstacles(50:75,50:75) = true;
obstacles(25:35,30:40) = true;
obstacles(25:35,60:80) = true;
figure;
imshow(~obstacles,"InitialMagnification",1000);
axis([0 xaxis 0 yaxis]);
axis xy;
axis on;
%PRM PARAMETERS
max_nodes_connect = 4;
max_connect_len = 40;
segments = 1;
max_nodes_grid = 30;
skipped = 0;
%PRM ALGO
nodes = 0; %Counter
map = zeros(size(obstacles)); %generate map
map(obstacles) = 1; %put the obstacles
Graph_connections = Inf(max_nodes_grid,max_nodes_connect 1); %rows = # nodes cols = ID and neighbors
while (nodes < max_nodes_grid)
node_x = randi(xaxis);
node_y = randi(yaxis);
if(map(node_y,node_x)==1 || map(node_y,node_x)==2)
continue;
end
nodes = nodes 1; %a valid node generated
map(node_y,node_x) = 2; %2 means there exists a node at that location
hold on
scatter(node_x,node_y,"red","filled")
%NODES TO CONNECT
nodes_to_connect = [];
distances = [];
for i= 1:numel(Graph_connections(:,1))
if(Graph_connections(i,1)==Inf)
break
end
[row,col] = ind2sub(size(map),Graph_connections(i,1));
%Check if within range
if(norm([node_y,node_x]-[row,col])>max_connect_len)
continue;
end
line_on_obstacle = check_obstacle(map,node_x,node_y,row,col);
%Check if obstacle thru line HAS TO BE WRITTEN
if(line_on_obstacle)
disp("Check Obstacle: " line_on_obstacle);
skipped = skipped 1;
continue;
end
nodes_to_connect = [nodes_to_connect, Graph_connections(i,1)];
distances = [distances; [Graph_connections(i,1),norm([node_y,node_x]-[row,col])]];
end
Graph_connections(nodes,1) = sub2ind(size(map),node_y,node_x);
if(size(distances)>0)
sorted_distances = sortrows(distances,2);
for i = 1:min(max_nodes_connect,size(sorted_distances,1))
Graph_connections(nodes,i 1) = sorted_distances(i,1);
[row,col] = ind2sub(size(map),sorted_distances(i,1));
if(line_on_obstacle==false)
disp("Line is not on obstacle")
hold on
plot([node_x,col],[node_y,row],"green","LineWidth",1.5);
continue;
else
disp("Line is on obstacle: " [node_x,col] " " [node_y,row]);
break;
end
end
disp("==========================")
end
end
function on_line = check_obstacle(map,node_x,node_y,row,col)
on_line = 0;
my_line = line([node_x,col],[node_y,row]);
line_spacing = max(abs(my_line.XData(1) - my_line.XData(2)) 1,abs(my_line.XData(1) - my_line.XData(2)) 1);
x_coordinates_line = round(linspace(my_line.XData(1),my_line.XData(2),line_spacing));
y_coordinates_line = round(linspace(my_line.YData(1),my_line.YData(2),line_spacing));
for i = 1:line_spacing
if(map(x_coordinates_line(i),y_coordinates_line(i))==1)
disp("ON OBSTACLE: " x_coordinates_line(i) " " y_coordinates_line(i));
on_line = true;
break;
end
end
end
The check_obstacle function is used to check if the points on the line are in the boundaries of obstacles. What am I missing here?
CodePudding user response：
close all;clear all;clc
format short
nx=100;ny=100; % grid size
[X,Y]=meshgrid(1:nx,1:ny);
Nobst=3 % amount obstacles
Nnet=30 % amount net points
Dmax=100 % net segment max length
% OBSTACLES
% define obstacles.
% Following are as defined in question
P1=[50 50; 75 50; 75 75; 50 75; 50 50];
P2=[25 30; 25 40; 35 40; 35 30; 25 30];
P3=[25 60; 25 80; 35 80;35 60;25 60];
% obstacle points all in one array
Pobst=[P1(:);P2(:);P3(:)];
Pobst=reshape(Pobst,[size(P1,1),size(P1,2),Nobst]);
% plot obstacles
hp=[]
figure(1)
ax=gca
hp=patch(squeeze(Pobst(:,1,:)),squeeze(Pobst(:,2,:)),[.5 .5 .5])
axis([1 nx 1 ny]);grid on
hp.EdgeAlpha=0;
ax.DataAspectRatio=[1 1 1]
hold(ax,'on')
% obstacle segments list : [x1 y1 x2 y2 d(X1,Y1)]
Lobst1=[]
for k=2:1:size(P1,1)
Lobst1=[Lobst1; [P1(k-1,:) P1(k,:) ((P1(k-1,1)-P1(k,1))^2 (P1(k-1,2)-P1(k,2))^2)^.5]];
end
Lobst2=[]
for k=2:1:size(P2,1)
Lobst2=[Lobst2; [P2(k-1,:) P2(k,:) ((P2(k-1,1)-P2(k,1))^2 (P2(k-1,2)-P2(k,2))^2)^.5]];
end
Lobst3=[]
for k=2:1:size(P3,1)
Lobst3=[Lobst3; [P3(k-1,:) P3(k,:) ((P3(k-1,1)-P3(k,1))^2 (P3(k-1,2)-P3(k,2))^2)^.5]];
end
Lobst=[Lobst1;Lobst2;Lobst3]
%% NETWORK
% all grid points outside obstacles
[in1,on1]=inpolygon(X(:),Y(:),P1(:,1),P1(:,2));
[in2,on2]=inpolygon(X(:),Y(:),P2(:,1),P2(:,2));
[in3,on3]=inpolygon(X(:),Y(:),P3(:,1),P3(:,2));
Xout=X(~in1 & ~in2 & ~in3);Yout=Y(~in1 & ~in2 & ~in3);
% plot(ax,Xout,Yout,'og','LineStyle','none') % check
% choose Nnet points outside obstacles
nP2=randi([1 numel(Xout)],Nnet,1);
Pnet=[Xout(nP2) Yout(nP2)];
plot(ax,Pnet(:,1),Pnet(:,2),'or','LineStyle','none')
% net segments list [x1 y1 x2 y2 d(X2,Y2) 0/1] 6th column [0 1] 1: draw 0: do not draw
nLnet=nchoosek([1:size(Pnet,1)],2);
Lnet=[Pnet(nLnet(:,1),1) Pnet(nLnet(:,1),2) ... % segment 1st point
Pnet(nLnet(:,2),1) Pnet(nLnet(:,2),2) ... % segment 2nd point
((Pnet(nLnet(:,1),1)-Pnet(nLnet(:,1),2)).^2 (Pnet(nLnet(:,2),1) Pnet(nLnet(:,2),2)).^2).^.5 ... % segment length
ones(size(nLnet,1),1)];
% check all net links are plotted
for k=1:1:size(Lnet,1)
plot(ax,[Lnet(k,1) Lnet(k,3)],[Lnet(k,2) Lnet(k,4)],'b');
hold(ax,'on')
end
% remove segments longer than Dmax
Lnet=sortrows(Lnet,5,'descend');
[~,n1]=max(Lnet(:,5)<=Dmax);
Lnet(1:n1-1,:)=[];
for k=1:1:size(Lnet,1)
plot(ax,[Lnet(k,1) Lnet(k,3)],[Lnet(k,2) Lnet(k,4)],'r');
hold(ax,'on')
end
%%
Redrawing and NOT removing net segments longer than Dmax
close all
hp=[]
figure(1)
ax=gca
hp=patch(squeeze(Pobst(:,1,:)),squeeze(Pobst(:,2,:)),[.5 .5 .5])
axis([1 nx 1 ny]);grid on
hp.EdgeAlpha=0;
ax.DataAspectRatio=[1 1 1]
hold(ax,'on')
plot(ax,Pnet(:,1),Pnet(:,2),'or','LineStyle','none')
Lnet=[Pnet(nLnet(:,1),1) Pnet(nLnet(:,1),2) ... % segment 1st point
Pnet(nLnet(:,2),1) Pnet(nLnet(:,2),2) ... % segment 2nd point
((Pnet(nLnet(:,1),1)-Pnet(nLnet(:,1),2)).^2 (Pnet(nLnet(:,2),1) Pnet(nLnet(:,2),2)).^2).^.5 ... % segment length
ones(size(nLnet,1),1)];
% check what pair segments intersect
for k2=1:1:size(Lnet,1)
allclear=ones(1,size(Lobst,1));
% allclear=zeros(1,size(Lobst,1));
for k1=1:1:size(Lobst,1)
% segments are contained in lines : check lines intersect
x1o=Lobst(k1,1);y1o=Lobst(k1,2);x2o=Lobst(k1,3);y2o=Lobst(k1,4);
x1n=Lnet(k2,1);y1n=Lnet(k2,2);x2n=Lnet(k2,3);y2n=Lnet(k2,4);
x1=x1o;x2=x2o;y1=y1o;y2=y2o;
x3=x1n;x4=x2n;y3=y1n;y4=y2n;
% t1 : x parameter
t1=((x1-x3)*(y3-y4)-(y1-y3)*(x3-x4))/((x1-x2)*(y3-y4)-(y1-y2)*(x3-x4));
xp=x1 t1*(x2-x1); % xp : crossing x coordinage
% u1 : y parameter
u1=-((x1-x2)*(y1-y3)-(y1-y2)*(x1-x3))/((x1-x2)*(y3-y4)-(y1-y2)*(x3-x4));
yp=y1 t1*(y2-y1); % yp : crossing y coordinate
% checks
plot(ax,x1o,y1o,'c*');plot(ax,x2o,y2o,'c*');plot(ax,[x1o x2o],[y1o y2o],'c')
plot(ax,x1n,y1n,'m*');plot(ax,x2n,y2n,'m*'); % plot(ax2,[x1n x2n],[y1n y2n],'m')
m1o=(y2o-y1o)/(x2o-x1o); % slopes
m2n=(y2n-y1n)/(x2n-x1n) ;
if (t1>=0 && t1<=1 && u1>=0 && u1<=1) && ...
(xp>=0 || xp<=nx || yp>=0 || yp<=ny)
allclear(k1)=0; % then do not plot this segment
end
end
if sum(allclear)==size(Lobst,1) %k2-th net segment hits no obstacles
plot(ax,x1n,y1n,'m*');plot(ax,x2n,y2n,'m*');plot(ax,[x1n x2n],[y1n y2n],'m')
elseif sum(allclear)~=size(Lobst,1)
Lnet(k2,end)=0;
end
end
Comments
1.- Note I have added format short because with format long there is decimal accretion right at the bottom of a few intersection points, that believe it or not, cause some false results.
2.- I like Torsen's explanation to find line segment intersections available here.
3.- There are faster ways to implement the main loop, but I make it go through all net-segment vs obstacle-element in case you may need it this way, to for instance count hits.
4.- There's also room for improvement in the way Lobst is generated.
5.- These lines
x1=x1o;x2=x2o;y1=y1o;y2=y2o;
x3=x1n;x4=x2n;y3=y1n;y4=y2n;
it's just an easy way to plug in formulation to already written lines, reducing amount variables is also left for next version.