% class for all vision functionality
classdef qrotor_vision < handle
properties
n = 1; % for testing with sample_vid
mlab_log = 'mlab_log.csv'; % log filename
mlab = 'mlab.csv' % current vals filename
pyth = 'pyth.csv' % altitude filename
frame_count % number of frames analyzed
gifname = 'testvid.gif'; % filename of gif
tstamp % current time (shared between log and preview frame)
fsize % [xres yres] % frame size in pixels
fps_loc % [xcoord ycoord] for data text on plots
fcenter % center of frame
vid % video object
b_preview = 0; % 1 shows preview plot, 0 does not
b_cam = 1; % camera used? 0 uses simulated images in a loop
framerate
stride_time = .5;
can_see % can the camera see the parrot
quit_search % set to quit the search function
altitude = 22; % of quadrotor
nom_altitude = 15; % nominal altitude, where the PID is tuned
bw_cutoff
trace_begin % point to start tracing boundary
past_centroid % [x y] location of previous centroid
centroid % [x y] location of latest centroid in pixels (starts in middle of cal frame)
c_int = 0; % [x y] integral value of centroid locations
c_der = 0; % [x y] derivative value of centroid (velocity)
kp = 5;
ki = .05;
kd = 3;
end
methods
% constructor
function self = qrotor_vision
% initialize csv
fid = fopen(self.mlab_log,'wt');
header = strcat('x correction,x_pro,x_int,x_der,',...
'y correction,y_pro,y_int,y_der,',...
'altitude,framerate,centroid_x,centroid_y\n');
fprintf(fid,header);
fclose(fid);
self.cam_cal
if self.b_cam == 1
self.centroid = self.fcenter; % center by default
else % test vid
self.centroid = [600 600];
self.fsize = [792 612];
self.fps_loc = [690 20];
self.fcenter = [396 306];
end
self.track_cent % starts infinite loop
end
% destructor
function delete(self)
imaqreset % disconnect camera
fclose('all'); % close all log files
end
% adjust cam resolution (also recreates vid object, out of necessity)
function res_adjust(self)
if self.altitude > 20
res = 'MJPG_1920x1080';
self.fsize = [1920 1080];
self.fps_loc = [1800 20];
self.fcenter = [960 540];
elseif self.altitude > 10
res = 'MJPG_1280x720';
self.fsize = [1280 720];
self.fps_loc = [1160 20];
self.fcenter = [640 360];
elseif self.altitude > 5
res = 'MJPG_640x360';
self.fsize = [640 360];
self.fps_loc = [540 20];
self.fcenter = [320 180];
else % good for at least 8 feet
res = 'MJPG_320x240';
self.fsize = [320 240];
self.fps_loc = [240 20];
self.fcenter = [160 120];
end
try
self.vid = videoinput('winvideo',1,res);
triggerconfig(self.vid,'manual');
start(self.vid);
catch
end
end % res_adjust
% centroid tracker
function track_cent(self) % tom needs to split this into multiple methods
while 1 % this is the main loop
update_duration = 0;
while update_duration < self.stride_time
tic
BW = self.parrot_boundary;
boundary = bwtraceboundary(BW,self.trace_begin,'N');
boundary_image = poly2mask(boundary(:,2),boundary(:,1),self.fsize(2),self.fsize(1));
% save last centroid location
try
self.past_centroid = self.centroid;
catch
end
% update centroid location
stat = regionprops(boundary_image,'centroid');
try
self.centroid = [stat(1).Centroid(1) stat(1).Centroid(2)];
%disp(strcat(num2str(stat(1).Centroid(1)),', ',num2str(stat(1).Centroid(2))));
catch
end
% run control
self.control_calcs
%self.n
if self.b_cam == 0
if self.n < 18
self.n = self.n + 1;
else
self.n = 1;
end
end
% timing stuff
update_duration = update_duration + toc;
self.framerate = 1/toc;
end
% show result
if self.b_preview == 1
clf
imshow(boundary_image);
hold on;
try
%centroid_marker = plot(stat(1).Centroid(1),stat(1).Centroid(2),'ro');
plot(stat(1).Centroid(1),stat(1).Centroid(2),'ro');
catch %err
%disp(err.identifier);
end
try
info_str = {strcat(num2str(self.framerate),' fps'),...
strcat(num2str(stat(1).Centroid(1)),', ',num2str(stat(1).Centroid(2))),...
self.tstamp{1}};
text(self.fps_loc(1),self.fps_loc(2),info_str,...
'BackgroundColor',[0 0 0],'Color',[1 1 1],'EdgeColor',[1 1 1]);
catch % leave out centroid coords if no centroid was found
info_str = {strcat(num2str(self.framerate),' fps'),self.tstamp{1}};
text(self.fps_loc(1),self.fps_loc(2),info_str,...
'BackgroundColor',[0 0 0],'Color',[1 1 1],'EdgeColor',[1 1 1]);
end
hold off;
drawnow
gif_frame = getframe(gcf); % grab both plots
im = frame2im(gif_frame);
[imind,cm] = rgb2ind(im,256);
imwrite(imind,cm,self.gifname,'gif','WriteMode','append');
end
%self.altitude = dlmread(self.pyth);
% see if altitude will change res_adjust, run if necessary?
end
end
% start or stop preview plot
function set.b_preview(self,bool)
if bool == 1
self.b_preview = 1;
elseif bool == 0
self.b_preview = 0;
else
warning('Input 0 or 1.');
end
end % b_preview
function cam_cal(self)
% set up camera
if self.b_cam == 1
self.res_adjust;
frame = getsnapshot(self.vid);
else
frame = imread('wm1.jpg');
end
warea = zeros(1,99); % white area
deriv_area = ones(1,95); % derivative of warea
for n = 1:99
BW = imcomplement(im2bw(frame,1-(n/100)));
if self.b_preview == 1
imshow(BW);
end
warea(n) = nnz(BW)/(1280*720);
if n > 1 && n < 95 % desired value should never be outside of these
deriv_area(n) = warea(n-1)-warea(n);
if warea(n) < .1 || warea(n) > .3
deriv_area(n) = 1; % restricts results
end
end
if self.b_preview == 1
drawnow
end
end
if self.b_preview == 1
% gif init
gif_frame = getframe(gcf); % grab both plots
im = frame2im(gif_frame);
[imind,cm] = rgb2ind(im,256);
imwrite(imind,cm,self.gifname,'gif','Loopcount',inf);
end
[~,index] = min(deriv_area);
%self.bw_cutoff = 1-((index+1)/100); % +5 was too dark
self.bw_cutoff = .35;
%warea(index); % white area
%self.altitude = 4;
%self.res_adjust;
end
function parrot_search(self) % scan edges of frame. if qrotor lost/on gps nav
self.quit_search = 0;
while 1;
self.can_see = 0; % can be checked periodically by autoflight?
% image capture
if self.b_cam == 1
frame = getsnapshot(self.vid); % camera
else
frame = imread(strcat('wm',num2str(self.n),'.jpg')); % canned image
end
% convert to inv binary
BW = imcomplement(im2bw(frame,self.bw_cutoff));
col = 0;
row = find(BW(:,col),1);
if isempty(row) && ~self.quit_search
col = self.fsize(2);
row = find(BW(:,col),1);
if isempty(row)
row = 0;
col = find(BW(row,:),1);
if isempty(col)
row = self.fsize(1);
col = find(BW(row,:),1);
else
self.trace_begin = [row,col];
break
end
else
self.trace_begin = [row,col];
break
end
else
self.trace_begin = [row,col];
break
end
self.can_see = 1;
end
end
function BW = parrot_boundary(self) % determine start point for boundary trace
% image capture
if self.b_cam == 1
frame = getsnapshot(self.vid); % camera
else
frame = imread(strcat('wm',num2str(self.n),'.jpg')); % canned image
self.can_see = 1;
end
% convert to inv binary
BW = imcomplement(im2bw(frame,self.bw_cutoff));
% search at last centroid
col = round(self.centroid(1));
row = find(BW(:,col),1);
if isempty(row)
% then try further in direction of last velocity (assume it was going fast)
% have x and y velocity saved. search col if y velocity greater, or search row if x velocity greater
% move the searched line according to c_der. how much?
[~, idx] = max(self.c_der); % determine if x or y velocity is greater
if idx == 1 % x velocity is greater, so row won't change as fast
row = round(self.centroid(2));
col = find(BW(row,:),1);
else % y velocity is greater, so col won't change as fast
col = round(self.centroid(1));
row = find(BW(:,col),1);
end
if isempty(row) && isempty(col) % still not found
% then try closer to center (assume correction started)
xdiff = (self.centroid(1) - self.fsize(1)/2);
ydiff = (self.centroid(2) - self.fsize(2)/2);
col = round(self.centroid(1) + xdiff/2);
row = find(BW(:,col),1);
if isempty(row)
self.can_see = 0;
self.parrot_search % so that it doesnt escape the frame
else
self.trace_begin = [row,col];
end
else
self.trace_begin = [row,col];
end
else
self.trace_begin = [row,col];
end
end
function csv_update(self,prop,control_out)
dlmwrite(self.mlab,...
[control_out(1) control_out(2) self.can_see self.centroid(1) self.centroid(2)]); % current vals only
csv_data = strcat(num2str(control_out(1)),',',num2str(self.centroid(1)),',',...
num2str(prop(1)),',',num2str(self.c_int(1)),',',num2str(self.c_der(1)),',',...
num2str(control_out(2)),',',num2str(self.centroid(2)),',',num2str(prop(2)),',',...
num2str(self.c_int(2)),',',num2str(self.c_der(2)),',',num2str(self.altitude),',',...
num2str(self.framerate),',',num2str(self.can_see),',',self.tstamp(1));
dlmwrite(self.mlab_log,csv_data,'-append','delimiter','','newline','pc'); % full log w/ history
end
function control_calcs(self) % PID controller
prop = self.fcenter - self.centroid;
prop(2) = -prop(2);
self.c_int = .8*(self.c_int + .6*prop); % has to stay updated
self.c_der = self.centroid - self.past_centroid; % also used elsewhere
self.c_der(1) = -self.c_der(1);
%alt_coeff = self.altitude/self.nom_altitude;
control_out = (self.kp*prop + self.ki*self.c_int + self.kd*self.c_der)...
./(self.fsize./10); %[x y]
disp(control_out);
self.tstamp = datestr(now,'MM:SS FFF');
self.tstamp = cellstr(self.tstamp);
self.csv_update(prop,control_out) % pass arguments instead of class vars
end
end
end