Tracking IR LEDs with Matlab
by
Programmed for my tricopter (micro air vehicle), http://shrediquette.blogspot.com/
My tricopter tracking process uses
several functions: There is one function (StartCam.m)
that sets up everything, starts a GUI and another function (independentTrack_timer.m).
The latter is the main function, which grabs the images from the camera and
executes a tracking algorithm. A GUI is used to enable "on the fly"
parameter editing. I will not include the code of the GUI because it is quite a
lot of code and in essence, it doesn't do anything important. It simply makes
PID parameter optimization more comfortable. Another function (figureclosingTrack.m) is used as the close request function
of the GUI. It will be executed when the user wants to stop tracking. This
function closes all connections (camera, serialport,
timer function).
I tried to comment everything in the source code. Comments are always on top of the commands that they describe.
This is the code of the .m file "StartCam.m". This function connects to a USB webcam, starts a timer and opens a serial port connection.
function StartCam (s,events)
%This block deletes all variables that were stored as application data
%(via "setappdata") in previous sessions. Restarting Matlab would have the
%same effect. The variables have to be deleted in order to enable a "clean" start.
try
app=getappdata(0);
appdatas = fieldnames(app);
for kA = 1:length(appdatas)
rmappdata(0,appdatas{kA});
end
catch ME
disp('rmappdata unsuccessful')
disp(ME.message)
end
%Clear the variables in the workspace
clear all
clc
%Find available video devices
info = imaqhwinfo('winvideo');
%Create a video object (camera device nr.2, resolution setting 5 = 640x480)
vid = videoinput('winvideo', 2, info.DeviceInfo(1,2).SupportedFormats{5});
%If we send a trigger signal, we want to receive 1 frame from the camera
set(vid,'FramesPerTrigger',1);
%Don't stop automatically after one frame was triggered
set(vid,'TriggerRepeat',inf);
%Manual means: The camera only records a frame if we send a trigger signal
triggerconfig(vid, 'Manual');
%Specific setting for my USB Camera
srcObj1 = get(vid, 'Source');
set(srcObj1(1), 'FrameRate', '30');
set(srcObj1(1), 'sharpness', 4);
set(srcObj1(1), 'BacklightCompensation', 'off');
set(srcObj1(1), 'ColorEnable', 'off');
set(srcObj1(1), 'ExposureMode', 'manual');
set(srcObj1(1), 'Exposure', 5);
%Connect to the camera...
start(vid);
%...and trigger a frame
trigger(vid)
%get the frame from the camera (the frame will not be used, just for "fun")
A = (getdata(vid,1,'uint8')); %get a frame from cam
%specific settings for my GUI application
setappdata(0,'timecount',0);
setappdata(0,'creep',0);
%I am calling "tic" here, because I will measure the speed of the analysis
%in a different function. "toc" without "tic" won't work of course...
tic
%start the GUI where you can set all parameters for the PID control loop
sensGUI;
%Get the "address" of the GUI
HsensGUI=getappdata(0,'sensGUI');
%Get a list of all elements in the GUI
handles=guihandles(HsensGUI);
%Some changes of the appearance of the GUI
set(HsensGUI, 'DockControls', 'off', 'menubar', 'none', 'name', 'Tritrack');
%When the user closes the GUI, some specfic actions have to be performed
%(e.g. closing the serialport, shutting down camera etc.) These actions will
%be found in the function "figureclosingTrack.m"
set(HsensGUI,'CloseRequestFcn',@figureclosingTrack)
%save the video object as appdata, so that other functions know which
%camera was selected.
setappdata(0,'vid', vid);
%save the list of elements of the GUI
setappdata(0,'handles',handles);
%Create a serialport with specific settings
s = serial('COM5','BaudRate',38400);
%After each command that was send via the serial port,
%a "carriage return" is added
set(s, 'Terminator', 'CR');
%open the serial port
fopen(s);
%save the serialport object
setappdata(0,'s',s);
%Create a timer object that will execute the function in
%"independentTrack_timer.m" as fast as possible with a pause of 0.01
%seconds between each execution. This little pause is necessary for matlab
%to perform other tasks. If we don't pause, Matlab will not accept any
%commands from the user anymore (that means you can't end the execution of
%the function independentTrack_timer anymore, nor can you exit Matlab.
t = timer('TimerFcn',@independentTrack_timer, 'Period', 0.01,'ExecutionMode', 'fixedSpacing');
%Start the timer
start(t)
%Open a window that shows a live preview of the camera image. First I
%wanted to have this preview inside my GUI. But it is much faster if you
%use the preview(vid) function. I don't know why.
preview(vid);
This is the code of the .m file "independentTrack_timer.m". This function is executed repeatedly as fast as possible. It gets images from the camera and performs motion tracking. Subsequently, it transfers data to the copter via the serial port.
%This function is a called in a loop as fast as possible (leaving 0.01
%seconds for Matlab to perform different tasks).
function independentTrack_timer(s, events)
%Measure the speed of calculations (only important for optimizing the
%analysis)
try
timecount=getappdata(0,'timecount');
if timecount < 20
setappdata(0,'timecount',timecount+1);
runningat=getappdata(0,'runningat');
else
runningat=round(1/(toc/20));
setappdata(0,'timecount',0);
tic
setappdata(0,'runningat',runningat);
end
catch ME
runningat=0;
disp (ME.message)
end
%Get the videoobject from the appdata
vid=getappdata(0,'vid');
%trigger a frame
trigger(vid);
%and read the frame from the camera
img1=getdata(vid,1,'uint8');
%empty the image buffer for the camera
flushdata(vid);
%in my case, images are transferred as YUV, that means that the grayscale
%information (that's the only thing I am interested in) can be found in
%(:,:,1)
img1=img1(:,:,1);
%we want to track 4 points
nrpoints=4;
%the threshold for the image binarization is set in the GUI
thresh=getappdata(0,'thresh');
try
%Binarize the image
A = im2bw(img1, (thresh/255));
%Find connected pixels in the image and give them a label
A = bwlabel(A,8);
%"Compress" the image
XMLABELS=sparse(A);
maxXM=max(A(:));
%Centroids: Finds the coordinates and the area of the
connected regions. The following
%code is much faster than "regionprops".
for i=1:maxXM
[ii,jj]=find(XMLABELS==i);
numberofpixels(i,1)=length(ii);
iind(i,1)=mean(ii);
jind(i,1)=mean(jj);
end
%Ainfo now contains a list with the positions and the size of all
%bright spots in the image
Ainfo=[jind iind numberofpixels];
%Sort this list, so that the biggest regions are on top of the list
Ainfo = sortrows(Ainfo,-3);
%How many bright spots did we find?
amountpeaks=size(Ainfo,1);
%Keep only the 4 biggest bright spots
Ainfo(nrpoints+1:end,:)=[];
%Calculate a matrix that contains the distances between individual
%bright spots.
distances=zeros(nrpoints,nrpoints);
for l=1:nrpoints
for q=1:nrpoints
distances(l,q)=sqrt((Ainfo(q,1)-Ainfo(l,1))^2+(Ainfo(q,2)-Ainfo(l,2))^2);
end
end
%Now you can do all sorts of calculations with the coordinates of the
%bright spots. In my case, I calculate the mean x&y displacement of all
%bright spots to get the position of the copter. By calculating the mean
%distance of the bright spots, you can find out the distance of the
%markers to the camera. I also calculate the yaw angle of the copter.
%When you have the information you need, you can calculate velocities and
%accelerations as well, in order to create a PID control loop.
%Here I am putting all parameters together....
senden_roll=((xsoll_send+xsolldiff_send+xsoll_a_send)*rollxinfluence + (ysoll_send+ysolldiff_send+ysoll_a_send)*-rollyinfluence)*startupgain*sensdist;
senden_nick=((xsoll_send+xsolldiff_send+xsoll_a_send)*nickxinfluence + (ysoll_send+ysolldiff_send+ysoll_a_send)*nickyinfluence)*startupgain*sensdist;
senden_pitch=-(psoll_send+psolldiff_send+psoll_a_send+creep)*startupgain*sensdist;
senden_yaw=(gsoll_send+gsolldiff_send+gsoll_a_send)*startupgain;
%The data is sent out via the serialport. Inside the copter, these values
%directly control the RPM of the motors. That means I am not modifying
%or filtering these values anymore.
fprintf(s,['A' int2str(senden_pitch)]);
fprintf(s,['B' int2str(senden_yaw)]);
fprintf(s,['C' int2str(senden_roll)]);
fprintf(s,['D' int2str(senden_nick)]);
catch
end
Finally, the code that is executed when the GUI is closed.
function figureclosingTrack (s,event)
disp('Figure was closed...')
%get information about the serialport, the GUI and the video connection
fighandle=getappdata(0,'sensGUI');
s=getappdata(0,'s');
vid=getappdata(0,'vid');
%first close the serial port
try
fclose(s)
disp(' --> serialport closed')
catch
disp('Error: Serial close')
end
%then shut down the preview window
try
stoppreview(vid)
closepreview(vid)
disp(' --> preview stopped')
catch
disp('Error: Preview stop')
end
%then stop and clear the timer function
try
stop(timerfind)
delete(timerfind)
disp(' --> timer stopped')
catch ME
disp('Error: Timer delete')
end
%Stop the camera object
try
stop(vid)
disp(' --> video stopped')
catch
disp('Error: Video stop')
end
%Clear the serialport
try
delete(s)
catch
disp('Error: Serial delete')
end
%Close the GUI
delete(fighandle)
clear all