MatLabscript uitbaldetectie
Jump to navigation
Jump to search
close all; clear camObj;
w=1;
d=0;
v=1
;
%% Setup
clear camObj
if w camObj=gigecam; end
coeffO = 5;
coeffC = 5;
centr_drone = [];
angle=0;
videoPlayer=vision.VideoPlayer('Name','Video','Position', [10, 10, 720, 480]);
foregrounddetect = vision.ForegroundDetector('NumGaussians', 3, ...
'NumTrainingFrames', 10, 'MinimumBackgroundRatio', 0.7);
nr=0;
%setup blob analyzer
blob_ball = vision.BlobAnalysis('BoundingBoxOutputPort', true, ...
'MajorAxisLengthOutputPort',true,'EccentricityOutputPort', true, 'CentroidOutputPort', true, ...
'MinimumBlobArea', 100, 'MaximumBlobArea',1000);
blob_drone = vision.BlobAnalysis('BoundingBoxOutputPort', true, ...
'CentroidOutputPort', true, 'MajorAxisLengthOutputPort',true,'MinorAxisLengthOutputPort',true,'OrientationOutputPort',true, ...
'MinimumBlobArea', 1000, 'MaximumBlobArea',4000);
if ~w videoReader = vision.VideoFileReader('topcam_drone.mp4'); end
%Coordinates of field corners and goal points
bottom_left=[168, 1049]; %[x, y]
bottom_right=[1699, 1008]; %[x, y]
upper_left=[155, 5]; %[x,y]
upper_right=[1685, 5]; %[x,y]
gl_up=370; %[y]
gl_down=670; %[y]
gr_up=350; %[y]
gr_down=630; %[y]
%Creating a formula for y, dependent on x
xdiff_down=bottom_right(1)-bottom_left(1);
ydiff_up=bottom_left(2)-bottom_right(2);
xdiff_left=bottom_left(1)-upper_left(1);
ydiff_left=bottom_left(2)-upper_left(2);
xdiff_right=upper_right(1)-bottom_right(1);
ydiff_right=upper_right(2)-bottom_right(2);
%Loading in sound file
my_sound = audioread('whistle.wav');
cheering=audioread('cheering.wav');
my_player = audioplayer(my_sound, 44100);
cheering = audioplayer(cheering, 44100)
%% Start Analysis
if ~v
img=snapshot(camObj);
step(videoPlayer,img);
end
centr_prev = [0;0];
angle_prev = 0;
if d
drone.takeoff();
end
while (isOpen(videoPlayer)) || nr==0
nr=nr+1;
if nr==5
release(foregrounddetect);
elseif nr>5 && nr<8 && d
moveForward(drone,0.5);
end
%Read one frame
if w
img=snapshot(camObj);
else
img=rgb2gray(step(videoReader));
end
%Filter out parts outside of the field
img = img(:,1:end-100);
img(img>200)=90;
foreground = foregrounddetect.step(img);
%foreground = imopen(foreground, strel('rectangle', [coeffO,coeffO]));
foreground = imclose(foreground, strel('rectangle', [coeffC, coeffC]));
if ~isempty(centr_drone)
centr_prev=centr_drone;
ball_prev = centr_ball;
angle_prev = angle;
else
centr_prev = [0;0];
angle_prev = angle;
ball_prev = [0;0];
end
%3a: detect blobs, return centroids, bounding boxes, eccentricity and
%diameter
[~,centr_ball,bbox_ball,diam,ecc] = step(blob_ball,foreground); %For ball
[~,centr_drone,bbox_drone,Ma,Mi,angle] = step(blob_drone,foreground); %For drone
%3b: maximize for most round object for ball
if ~isempty(centr_ball)
[ecmin,I] = min(ecc,[],1);
centr_ball = centr_ball(I,:);
diam = diam(I);
else
centr_ball = [0 0]; diam = 0;
ecmin=1e10;
end
if ecmin>0.7
centr_ball = [0 0]; diam = 0;
%disp('Noise!');
else
%disp('Ball detected');
end
%% Line Detection
down_line=(bottom_left(2)+(ydiff_up/xdiff_down)*bottom_left(1))-(ydiff_up/xdiff_down)*centr_ball(1);
left_line=(upper_left(1)+(xdiff_left/ydiff_left)*upper_left(2))+(xdiff_left/ydiff_left)*centr_ball(2);
right_line=(bottom_right(1)+(xdiff_right/ydiff_right)*bottom_right(2))+(xdiff_right/ydiff_right)*centr_ball(2);
%IF loop (if the ball is above the top line, play the whistle)
if ball_prev(1)>0 && ball_prev(2)>0 && ball_prev(2)<200 && centr_ball(1)==0 && centr_ball(2)==0
if ~isplaying(my_player)
play(my_player);
disp('Out! Top ');
end
end
if centr_ball(1)~=0 && centr_ball(2)~=0
%IF loop (if the ball is above the bottom line, play the whistle)
if centr_ball(2)>=down_line
if ~isplaying(my_player)
play(my_player);
disp('Out! Down');
end
end %out of field down line
%IF loop (if the ball is in the left goal, play the GOLGOLGOL-tune)
if (centr_ball(1)<=left_line && centr_ball(2)>=gl_up && centr_ball(1)<=left_line && centr_ball(2)<=gl_down)
if ~isplaying(cheering)
play(cheering);
disp('GOAL! - Left');
end
elseif centr_ball(1)==0
[];
end %goal left half
%IF loop (if the ball is in the right goal, play the GOLGOLGOL-tune)
if (centr_ball(1)>=right_line && centr_ball(2)>=gr_up && centr_ball(1)>=right_line && centr_ball(2)<=gr_down)
if ~isplaying(cheering)
play(cheering);
disp('GOAL! - Right');
end
elseif centr_ball(1)==0
[];
end %goal right half
%IF loop (if the ball is right of the right line, play the whistle)
if (centr_ball(1)>=right_line && centr_ball(2)<=gr_up) || (centr_ball(1)>=right_line && centr_ball(2)>=gr_down)
if ~isplaying(my_player)
play(my_player);
disp('Out! Right');
end
elseif centr_ball(1)==0
[];
end %out of field right
%IF loop (if the ball is left of the left line, play the whistle)
if (centr_ball(1)<=left_line && centr_ball(2)<=gl_up) || (centr_ball(1)<=left_line && centr_ball(2)>=gl_down)
if ~isplaying(my_player)
play(my_player);
disp('Out! Left')
end
elseif centr_ball(1)==0
[];
end %out of field left
end
test = [0 0];
test(1) = centr_drone(1) + 200*cosd(angle);
test(2) = centr_drone(2) + 200*sind(angle);
if v
foreground = insertShape(1.*foreground, 'Rectangle', bbox_drone);
foreground = insertShape(1.*foreground, 'FilledCircle', [centr_ball diam./2],'SmoothEdges',false,'Color', [255,0,0]);
%foreground = insertShape(1.*foreground, 'Line', [centr_drone,test]);
%Displaying the yellow lines for clarification
img = insertShape(1.*img, 'Line',[bottom_left,bottom_right],'LineWidth', 5);
img = insertShape(1.*img, 'Line',[upper_left,bottom_left],'LineWidth', 5);
img = insertShape(1.*img, 'Line',[upper_right,bottom_right],'LineWidth', 5);
step(videoPlayer,foreground);
end
end
if d
drone.land();
end
release(videoPlayer);
Terug naar: PRE2015_2_Groep2