Matlabscript Botsingsdetectie

From Control Systems Technology Group

(Difference between revisions)
Jump to: navigation, search
(Created page with '<code> function Collision_detection() clc % Create System objects used for reading video, detecting moving objects, % and displaying the results. obj = setupSystemObjects(); tra…')
Line 1: Line 1:
<code>
<code>
 +
function Collision_detection()
function Collision_detection()
-
clc
+
    clc
-
% Create System objects used for reading video, detecting moving objects,
+
    % Create System objects used for reading video, detecting moving objects,
-
% and displaying the results.
+
    % and displaying the results.
-
obj = setupSystemObjects();
+
    obj = setupSystemObjects();
-
 
+
.
-
tracks = initializeTracks(); % Create an empty array of tracks.
+
    tracks = initializeTracks(); % Create an empty array of tracks.
-
 
+
.
-
nextId = 1; % ID of the next track
+
    nextId = 1; % ID of the next track
-
 
+
.
-
% Detect moving objects, and track them across video frames.
+
    % Detect moving objects, and track them across video frames.
-
fr=0;
+
    fr=0;
-
disp('Starting...');
+
    disp('Starting...');
-
 
+
.
-
f = step(obj.reader);
+
    f = step(obj.reader);
-
obj.videoPlayer.step(f); release(obj.videoPlayer);
+
    obj.videoPlayer.step(f); release(obj.videoPlayer);
-
 
+
.
-
Lastpos = [0 0];
+
    Lastpos = [0 0];
-
Currentpos = [0 0];
+
    Currentpos = [0 0];
-
colvar = 1;
+
    colvar = 1;
-
SHOW = 1;  %turn on/off message box
+
    SHOW = 1;  %turn on/off message box
-
 
+
.
-
while fr < 41 && isOpen(obj.videoPlayer)
+
    while fr < 41 && isOpen(obj.videoPlayer)
-
    fr;
+
        fr;
-
    frame = readFrame();
+
        frame = readFrame();
-
    [centroids, bboxes, mask] = detectObjects(frame, blob);
+
        [centroids, bboxes, mask] = detectObjects(frame, blob);
-
    predictNewLocationsOfTracks();
+
        predictNewLocationsOfTracks();
-
    [assignments, ~ , unassignedDetections] = ...
+
        [assignments, ~ , unassignedDetections] = ...
-
      detectionToTrackAssignment();
+
            detectionToTrackAssignment();
-
    updateAssignedTracks();  %position
+
        updateAssignedTracks();  %position
-
    createNewTracks();
+
        createNewTracks();
-
    displayTrackingResults();
+
        displayTrackingResults();
-
    fr=fr+1;
+
        fr=fr+1;
-
end
+
    end
-
 
+
.
-
release(obj.videoPlayer);
+
    release(obj.videoPlayer);
-
obj.videoPlayer.hide();
+
    obj.videoPlayer.hide();
-
close all;
+
    close all;
-
 
+
.
-
 
+
    %% Create System Objects
-
%% Create System Objects
+
    % Create System objects used for reading the video frames, detecting
-
% Create System objects used for reading the video frames, detecting
+
    % colored objects, and displaying results.
-
% colored objects, and displaying results.
+
.
-
 
+
     function obj = setupSystemObjects()
     function obj = setupSystemObjects()
         % Initialize Video
         % Initialize Video
Line 50: Line 50:
         % can be analyzed yet
         % can be analyzed yet
         obj.reader = vision.VideoFileReader('botsing1.mp4');
         obj.reader = vision.VideoFileReader('botsing1.mp4');
-
       
+
.       
         % Create a video player
         % Create a video player
         obj.videoPlayer = vision.VideoPlayer('Position', [200, 400, 700, 400]);
         obj.videoPlayer = vision.VideoPlayer('Position', [200, 400, 700, 400]);
-
       
+
.
         % Create System objects based on movement and blob analysis
         % Create System objects based on movement and blob analysis
         obj.detector = vision.ForegroundDetector('NumGaussians', 3, ...
         obj.detector = vision.ForegroundDetector('NumGaussians', 3, ...
             'NumTrainingFrames', 5, 'MinimumBackgroundRatio', 0.8);
             'NumTrainingFrames', 5, 'MinimumBackgroundRatio', 0.8);
-
 
+
.
         blob = vision.BlobAnalysis('BoundingBoxOutputPort', true, 'ExcludeBorderBlobs',true, ...
         blob = vision.BlobAnalysis('BoundingBoxOutputPort', true, 'ExcludeBorderBlobs',true, ...
             'MajorAxisLengthOutputPort',true,'EccentricityOutputPort', true, 'CentroidOutputPort', true, ...
             'MajorAxisLengthOutputPort',true,'EccentricityOutputPort', true, 'CentroidOutputPort', true, ...
             'MinimumBlobArea', 2100, 'MaximumBlobArea',50000);
             'MinimumBlobArea', 2100, 'MaximumBlobArea',50000);
     end
     end
-
 
+
.
-
%% Initialize Tracks
+
    %% Initialize Tracks
-
% The structure contains the following fields:
+
    % The structure contains the following fields:
-
%
+
    %
-
% * |id| :                        the integer ID of the track
+
    % * |id| :                        the integer ID of the track
-
% * |bbox| :                      the current bounding box of the object; used
+
    % * |bbox| :                      the current bounding box of the object; used
-
%                                for display
+
    %                                for display
-
% * |kalmanFilter| :              a Kalman filter object used for motion-based
+
    % * |kalmanFilter| :              a Kalman filter object used for motion-based
-
%                                tracking
+
    %                                tracking
-
% * |age| :                      the number of frames since the track was first
+
    % * |age| :                      the number of frames since the track was first
-
%                                detected
+
    %                                detected
-
% * |totalVisibleCount| :        the total number of frames in which the track
+
    % * |totalVisibleCount| :        the total number of frames in which the track
-
%                                was detected (visible)
+
    %                                was detected (visible)
-
% * |consecutiveInvisibleCount| : the number of consecutive frames for  
+
    % * |consecutiveInvisibleCount| : the number of consecutive frames for  
-
%                                which the track was not detected (invisible).
+
    %                                which the track was not detected (invisible).
-
%                                This results in deleting the track if the
+
    %                                This results in deleting the track if the
-
%                                threshold is reached
+
    %                                threshold is reached
-
 
+
.
     function tracks = initializeTracks()
     function tracks = initializeTracks()
         % create an empty array of tracks
         % create an empty array of tracks
Line 90: Line 90:
             'consecutiveInvisibleCount', {});
             'consecutiveInvisibleCount', {});
     end
     end
-
 
+
.
-
%% Read a Video Frame
+
    %% Read a Video Frame
-
% Read the next video frame from the video file.
+
    % Read the next video frame from the video file.
     function frame = readFrame()
     function frame = readFrame()
         frame = obj.reader.step();
         frame = obj.reader.step();
         %frame = snapshot(cam);
         %frame = snapshot(cam);
     end
     end
-
 
+
.
-
%% Detect Objects
+
    %% Detect Objects
-
% The |detectObjects| function returns the centroids and the bounding boxes
+
    % The |detectObjects| function returns the centroids and the bounding boxes
-
% of the detected objects. It also returns the binary mask, which has the  
+
    % of the detected objects. It also returns the binary mask, which has the  
-
% same size as the input frame. Pixels with a value of 1 correspond to the
+
    % same size as the input frame. Pixels with a value of 1 correspond to the
-
% foreground, and pixels with a value of 0 correspond to the background.  
+
    % foreground, and pixels with a value of 0 correspond to the background.  
-
 
+
.
     function [centroids, bboxes, mask] = detectObjects(frame, blob)
     function [centroids, bboxes, mask] = detectObjects(frame, blob)
-
       
+
.     
     % Use color to identify turtles from each team. Only these colors will
     % Use color to identify turtles from each team. Only these colors will
     % be taken into account. A distinctive top color results in more  
     % be taken into account. A distinctive top color results in more  
Line 111: Line 111:
     Red = frame*255;
     Red = frame*255;
     Red = Red(:,:,1)>Red(:,:,2)*2 & Red(:,:,1)>Red(:,:,2)*2;
     Red = Red(:,:,1)>Red(:,:,2)*2 & Red(:,:,1)>Red(:,:,2)*2;
-
   
+
.
     mask = obj.detector.step(frame);
     mask = obj.detector.step(frame);
   % Apply morphological operations to remove noise and fill in holes.
   % Apply morphological operations to remove noise and fill in holes.
Line 117: Line 117:
     mask = imclose(mask, strel('rectangle', [15, 15]));  
     mask = imclose(mask, strel('rectangle', [15, 15]));  
     mask = imfill(mask, 'holes');
     mask = imfill(mask, 'holes');
-
 
+
.
   % detect blobs, return centroids, bounding boxes, eccentricity and diameter
   % detect blobs, return centroids, bounding boxes, eccentricity and diameter
   [~,centroids,bboxes,diam,ecc] = step(blob,Red);               
   [~,centroids,bboxes,diam,ecc] = step(blob,Red);               
-
 
+
.
   % maximize for most round object
   % maximize for most round object
   if ~isempty(centroids)     
   if ~isempty(centroids)     
Line 135: Line 135:
         diam = [];
         diam = [];
   end
   end
-
 
+
.
     end
     end
-
%% Predict New Locations of Existing Tracks
+
    %% Predict New Locations of Existing Tracks
-
% Use the Kalman filter to predict the centroid of each track in the
+
    % Use the Kalman filter to predict the centroid of each track in the
-
% current frame, and update its bounding box accordingly.
+
    % current frame, and update its bounding box accordingly.
-
 
+
.
     function predictNewLocationsOfTracks()
     function predictNewLocationsOfTracks()
       for i = 1:length(tracks)
       for i = 1:length(tracks)
             bbox = tracks(i).bbox;
             bbox = tracks(i).bbox;
-
           
+
.
             % Predict the current location of the track.
             % Predict the current location of the track.
             predictedCentroid = predict(tracks(i).kalmanFilter);
             predictedCentroid = predict(tracks(i).kalmanFilter);
Line 153: Line 153:
       end
       end
     end
     end
-
 
+
.
-
%% Assign Detections to Tracks
+
    %% Assign Detections to Tracks
-
% Assigning object detections in the current frame to existing tracks is
+
    % Assigning object detections in the current frame to existing tracks is
-
% done by minimizing cost. The cost is defined as the negative
+
    % done by minimizing cost. The cost is defined as the negative
-
% log-likelihood of a detection corresponding to a track.   
+
    % log-likelihood of a detection corresponding to a track.   
-
 
+
.
     function [assignments, unassignedTracks, unassignedDetections] = ...
     function [assignments, unassignedTracks, unassignedDetections] = ...
             detectionToTrackAssignment()
             detectionToTrackAssignment()
-
       
+
.
         nTracks = length(tracks);
         nTracks = length(tracks);
         nDetections = size(centroids, 1);
         nDetections = size(centroids, 1);
-
       
+
.
         % Compute the cost of assigning each detection to each track.
         % Compute the cost of assigning each detection to each track.
         cost = zeros(nTracks, nDetections);
         cost = zeros(nTracks, nDetections);
Line 170: Line 170:
             cost(1, :) = distance(tracks(1).kalmanFilter, centroids);
             cost(1, :) = distance(tracks(1).kalmanFilter, centroids);
         end
         end
-
       
+
.
         % Solve the assignment problem.
         % Solve the assignment problem.
         costOfNonAssignment = 20;
         costOfNonAssignment = 20;
Line 176: Line 176:
             assignDetectionsToTracks(cost, costOfNonAssignment);
             assignDetectionsToTracks(cost, costOfNonAssignment);
     end
     end
-
 
+
.
-
%% Update Assigned Tracks
+
    %% Update Assigned Tracks
-
% The |updateAssignedTracks| function updates each assigned track with the
+
    % The |updateAssignedTracks| function updates each assigned track with the
-
% corresponding detection. It calls the |correct| method of
+
    % corresponding detection. It calls the |correct| method of
-
% |vision.KalmanFilter| to correct the location estimate. Next, it stores
+
    % |vision.KalmanFilter| to correct the location estimate. Next, it stores
-
% the new bounding box, and increases the age of the track and the total
+
    % the new bounding box, and increases the age of the track and the total
-
% visible count by 1. Finally, the function sets the invisible count to 0.
+
    % visible count by 1. Finally, the function sets the invisible count to 0.
-
 
+
.
     function updateAssignedTracks()
     function updateAssignedTracks()
         numAssignedTracks = size(assignments, 1);
         numAssignedTracks = size(assignments, 1);
Line 191: Line 191:
             centroid = centroids(detectionIdx, :);
             centroid = centroids(detectionIdx, :);
             bbox = bboxes(detectionIdx, :);
             bbox = bboxes(detectionIdx, :);
-
           
+
.
             % Correct the estimate of the object's location
             % Correct the estimate of the object's location
             % using the new detection. This will give the current position
             % using the new detection. This will give the current position
Line 201: Line 201:
             % Update track's age.
             % Update track's age.
             tracks(trackIdx).age = tracks(trackIdx).age + 1;
             tracks(trackIdx).age = tracks(trackIdx).age + 1;
-
           
+
.
             % Update visibility.
             % Update visibility.
             tracks(trackIdx).totalVisibleCount = ...
             tracks(trackIdx).totalVisibleCount = ...
Line 210: Line 210:
         end
         end
     end
     end
-
 
+
.
-
%% Create New Tracks
+
    %% Create New Tracks
-
% Create new tracks from unassigned detections. Assume that any unassigned
+
    % Create new tracks from unassigned detections. Assume that any unassigned
-
% detection is a start of a new track. In practice, you can use other cues
+
    % detection is a start of a new track. In practice, you can use other cues
-
% to eliminate noisy detections, such as size, location, or appearance.
+
    % to eliminate noisy detections, such as size, location, or appearance.
-
 
+
.
     function createNewTracks()
     function createNewTracks()
         centroids = centroids(unassignedDetections, :);
         centroids = centroids(unassignedDetections, :);
         bboxes = bboxes(unassignedDetections, :);
         bboxes = bboxes(unassignedDetections, :);
-
       
+
.
         for i = 1:size(centroids, 1)
         for i = 1:size(centroids, 1)
-
           
+
.
             centroid = centroids(i,:);
             centroid = centroids(i,:);
             bbox = bboxes(i, :);
             bbox = bboxes(i, :);
-
           
+
.
             % Create a Kalman filter object.
             % Create a Kalman filter object.
             kalmanFilter = configureKalmanFilter('ConstantVelocity', ...
             kalmanFilter = configureKalmanFilter('ConstantVelocity', ...
                 centroid, [200, 50], [100, 25], 200);
                 centroid, [200, 50], [100, 25], 200);
-
           
+
.
             % Create a new track.
             % Create a new track.
             newTrack = struct(...
             newTrack = struct(...
Line 237: Line 237:
                 'totalVisibleCount', 4, ...
                 'totalVisibleCount', 4, ...
                 'consecutiveInvisibleCount', 0);
                 'consecutiveInvisibleCount', 0);
-
           
+
.
             % Add it to the array of tracks.
             % Add it to the array of tracks.
             tracks(end + 1) = newTrack;
             tracks(end + 1) = newTrack;
-
           
+
.
             % Increment the next id.
             % Increment the next id.
             nextId = nextId + 1;
             nextId = nextId + 1;
         end
         end
     end
     end
-
 
+
.
-
%% Display Tracking Results
+
    %% Display Tracking Results
-
% The |displayTrackingResults| function draws a bounding box and label ID  
+
    % The |displayTrackingResults| function draws a bounding box and label ID  
-
% for each track on the video frame and the foreground mask. It then  
+
    % for each track on the video frame and the foreground mask. It then  
-
% displays the frame and the mask in their respective video players.  
+
    % displays the frame and the mask in their respective video players.  
-
 
+
.
     function displayTrackingResults()
     function displayTrackingResults()
         % Convert the frame and the mask to uint8 RGB.
         % Convert the frame and the mask to uint8 RGB.
         frame = im2uint8(frame);
         frame = im2uint8(frame);
         mask = uint8(repmat(mask, [1, 1, 3])) .* 255;
         mask = uint8(repmat(mask, [1, 1, 3])) .* 255;
-
       
+
.
         minVisibleCount = 8;
         minVisibleCount = 8;
         if ~isempty(tracks)
         if ~isempty(tracks)
-
             
+
.
             % Noisy detections tend to result in short-lived tracks.
             % Noisy detections tend to result in short-lived tracks.
             % Only display tracks that have been visible for more than  
             % Only display tracks that have been visible for more than  
Line 265: Line 265:
                 [tracks(:).totalVisibleCount] > minVisibleCount;
                 [tracks(:).totalVisibleCount] > minVisibleCount;
             reliableTracks = tracks(reliableTrackInds);
             reliableTracks = tracks(reliableTrackInds);
-
           
+
.
             % Display the objects. If an object has not been detected
             % Display the objects. If an object has not been detected
             % in this frame, display its predicted bounding box.
             % in this frame, display its predicted bounding box.
Line 271: Line 271:
                 % Get bounding boxes.
                 % Get bounding boxes.
                 bboxes = cat(1, reliableTracks.bbox);
                 bboxes = cat(1, reliableTracks.bbox);
-
               
+
.
                 % Get ids.
                 % Get ids.
                 ids = int32([reliableTracks(:).id]);
                 ids = int32([reliableTracks(:).id]);
-
               
+
.
                 % Create labels for objects indicating the ones for  
                 % Create labels for objects indicating the ones for  
                 % which we display the predicted rather than the actual  
                 % which we display the predicted rather than the actual  
Line 284: Line 284:
                 isPredicted(predictedTrackInds) = {' predicted'};
                 isPredicted(predictedTrackInds) = {' predicted'};
                 labels = strcat(labels, isPredicted);
                 labels = strcat(labels, isPredicted);
-
             
+
.
                 labels = 'Red Ball';
                 labels = 'Red Ball';
                 % Draw the objects on the frame.
                 % Draw the objects on the frame.
                 frame = insertObjectAnnotation(frame, 'rectangle', ...
                 frame = insertObjectAnnotation(frame, 'rectangle', ...
                     bboxes, labels);
                     bboxes, labels);
-
               
+
.
                 % Draw the objects on the mask.
                 % Draw the objects on the mask.
                 mask = insertObjectAnnotation(mask, 'rectangle', ...
                 mask = insertObjectAnnotation(mask, 'rectangle', ...
Line 295: Line 295:
             end
             end
         end
         end
-
       
+
.
         % Display the mask and the frame.       
         % Display the mask and the frame.       
         obj.videoPlayer.step(frame);
         obj.videoPlayer.step(frame);
     end
     end
-
 
+
.
-
%% Store previous location
+
    %% Store previous location
-
% The |saveposition| function stores the location of the previous frame to
+
    % The |saveposition| function stores the location of the previous frame to
-
% allow for the calculation of a direction vector created from consecutive
+
    % allow for the calculation of a direction vector created from consecutive
-
% frames
+
    % frames
-
 
+
.
     function saveposition()
     function saveposition()
         if fr <2
         if fr <2
Line 312: Line 312:
         end
         end
     end
     end
-
     
+
.
-
%% Check for a collision
+
    %% Check for a collision
-
% The |checkcollision| function checks every frame if there is a sudden
+
    % The |checkcollision| function checks every frame if there is a sudden
-
% change in the direction compared to its previous. If more elaborate  
+
    % change in the direction compared to its previous. If more elaborate  
-
% collision rules apply, one can look to find the acceleration
+
    % collision rules apply, one can look to find the acceleration
-
 
+
.
     function checkcollision()
     function checkcollision()
         if ~isempty(Lastpos) && fr >17  %% Bounding box drawn
         if ~isempty(Lastpos) && fr >17  %% Bounding box drawn
Line 330: Line 330:
         end
         end
     end
     end
 +
.
 +
end
-
%end of script
 
-
end
 
</code>
</code>
 +
 +
A link to the matlab code can be found here: [http://www.example.com link title]

Revision as of 23:43, 12 January 2016

function Collision_detection()

   clc
   % Create System objects used for reading video, detecting moving objects,
   % and displaying the results.
   obj = setupSystemObjects();
.
   tracks = initializeTracks(); % Create an empty array of tracks.
.
   nextId = 1; % ID of the next track
.
   % Detect moving objects, and track them across video frames.
   fr=0;
   disp('Starting...');
.
   f = step(obj.reader);
   obj.videoPlayer.step(f); release(obj.videoPlayer);
.
   Lastpos = [0 0];
   Currentpos = [0 0];
   colvar = 1;
   SHOW = 1;  %turn on/off message box
.
   while fr < 41 && isOpen(obj.videoPlayer)
       fr;
       frame = readFrame();
       [centroids, bboxes, mask] = detectObjects(frame, blob);
       predictNewLocationsOfTracks();
       [assignments, ~ , unassignedDetections] = ...
           detectionToTrackAssignment();
       updateAssignedTracks();  %position
       createNewTracks();
       displayTrackingResults();
       fr=fr+1;
   end
.
   release(obj.videoPlayer);
   obj.videoPlayer.hide();
   close all;
.
   %% Create System Objects
   % Create System objects used for reading the video frames, detecting
   % colored objects, and displaying results.
.
   function obj = setupSystemObjects()
       % Initialize Video
       % Create objects for reading a video from a file, drawing the tracked
       % objects in each frame, and playing the video. No live camera data
       % can be analyzed yet
       obj.reader = vision.VideoFileReader('botsing1.mp4');
.        
       % Create a video player
       obj.videoPlayer = vision.VideoPlayer('Position', [200, 400, 700, 400]);
.
       % Create System objects based on movement and blob analysis
       obj.detector = vision.ForegroundDetector('NumGaussians', 3, ...
           'NumTrainingFrames', 5, 'MinimumBackgroundRatio', 0.8);
.
       blob = vision.BlobAnalysis('BoundingBoxOutputPort', true, 'ExcludeBorderBlobs',true, ...
           'MajorAxisLengthOutputPort',true,'EccentricityOutputPort', true, 'CentroidOutputPort', true, ...
           'MinimumBlobArea', 2100, 'MaximumBlobArea',50000);
   end
.
   %% Initialize Tracks
   % The structure contains the following fields:
   %
   % * |id| :                        the integer ID of the track
   % * |bbox| :                      the current bounding box of the object; used
   %                                 for display
   % * |kalmanFilter| :              a Kalman filter object used for motion-based
   %                                 tracking
   % * |age| :                       the number of frames since the track was first
   %                                 detected
   % * |totalVisibleCount| :         the total number of frames in which the track
   %                                 was detected (visible)
   % * |consecutiveInvisibleCount| : the number of consecutive frames for 
   %                                 which the track was not detected (invisible).
   %                                 This results in deleting the track if the
   %                                 threshold is reached
.
   function tracks = initializeTracks()
       % create an empty array of tracks
       tracks = struct(...
           'id', {}, ...
           'bbox', {}, ...
           'kalmanFilter', {}, ...
           'age', {}, ...
           'totalVisibleCount', {}, ...
           'consecutiveInvisibleCount', {});
   end
.
   %% Read a Video Frame
   % Read the next video frame from the video file.
   function frame = readFrame()
       frame = obj.reader.step();
       %frame = snapshot(cam);
   end
.
   %% Detect Objects
   % The |detectObjects| function returns the centroids and the bounding boxes
   % of the detected objects. It also returns the binary mask, which has the 
   % same size as the input frame. Pixels with a value of 1 correspond to the
   % foreground, and pixels with a value of 0 correspond to the background. 
.
   function [centroids, bboxes, mask] = detectObjects(frame, blob)
.      
   % Use color to identify turtles from each team. Only these colors will
   % be taken into account. A distinctive top color results in more 
   % accurate tracking.
   Red = frame*255;
   Red = Red(:,:,1)>Red(:,:,2)*2 & Red(:,:,1)>Red(:,:,2)*2;
.
   mask = obj.detector.step(frame);
 % Apply morphological operations to remove noise and fill in holes.
   mask = imopen(mask, strel('rectangle', [9,9]));
   mask = imclose(mask, strel('rectangle', [15, 15])); 
   mask = imfill(mask, 'holes');
.
  % detect blobs, return centroids, bounding boxes, eccentricity and diameter
  [~,centroids,bboxes,diam,ecc] = step(blob,Red);              
.
  % maximize for most round object
  if ~isempty(centroids)    
       [~,I] = max(ecc,[],1);
       bboxes = bboxes(I,:);
       centroids = centroids(I,:);
       diam = diam(I);
  end
  %check if max is indeed round (i.e. if anything useful detected)
  if ecc > 1
       ecc = [];
       centroids = [];
       bboxes = [];
       diam = [];
  end
.
   end
   %% Predict New Locations of Existing Tracks
   % Use the Kalman filter to predict the centroid of each track in the
   % current frame, and update its bounding box accordingly.
.
   function predictNewLocationsOfTracks()
     for i = 1:length(tracks)
           bbox = tracks(i).bbox;
.
           % Predict the current location of the track.
           predictedCentroid = predict(tracks(i).kalmanFilter);
           % Shift the bounding box so that its center is at 
           % the predicted location.
           predictedCentroid = int32(predictedCentroid) - bbox(3:4) / 2;
           tracks(i).bbox = [predictedCentroid, bbox(3:4)];
     end
   end
.
   %% Assign Detections to Tracks
   % Assigning object detections in the current frame to existing tracks is
   % done by minimizing cost. The cost is defined as the negative
   % log-likelihood of a detection corresponding to a track.  
.
   function [assignments, unassignedTracks, unassignedDetections] = ...
           detectionToTrackAssignment()
.
       nTracks = length(tracks);
       nDetections = size(centroids, 1);
.
       % Compute the cost of assigning each detection to each track.
       cost = zeros(nTracks, nDetections);
       if nTracks>0
           cost(1, :) = distance(tracks(1).kalmanFilter, centroids);
       end
.
       % Solve the assignment problem.
       costOfNonAssignment = 20;
       [assignments, unassignedTracks, unassignedDetections] = ...
           assignDetectionsToTracks(cost, costOfNonAssignment);
   end
.
   %% Update Assigned Tracks
   % The |updateAssignedTracks| function updates each assigned track with the
   % corresponding detection. It calls the |correct| method of
   % |vision.KalmanFilter| to correct the location estimate. Next, it stores
   % the new bounding box, and increases the age of the track and the total
   % visible count by 1. Finally, the function sets the invisible count to 0.
.
   function updateAssignedTracks()
       numAssignedTracks = size(assignments, 1);
       for i = 1:numAssignedTracks
           trackIdx = assignments(i, 1);
           detectionIdx = assignments(i, 2);
           centroid = centroids(detectionIdx, :);
           bbox = bboxes(detectionIdx, :);
.
           % Correct the estimate of the object's location
           % using the new detection. This will give the current position
           Currentpos = correct(tracks(trackIdx).kalmanFilter, centroid);
           % Replace predicted bounding box with detected
           % bounding box.
           tracks(trackIdx).bbox = bbox;
           
           % Update track's age.
           tracks(trackIdx).age = tracks(trackIdx).age + 1;
.
           % Update visibility.
           tracks(trackIdx).totalVisibleCount = ...
               tracks(trackIdx).totalVisibleCount + 1;
           tracks(trackIdx).consecutiveInvisibleCount = 0;
              checkcollision();
              saveposition();
       end
   end
.
   %% Create New Tracks
   % Create new tracks from unassigned detections. Assume that any unassigned
   % detection is a start of a new track. In practice, you can use other cues
   % to eliminate noisy detections, such as size, location, or appearance.
.
   function createNewTracks()
       centroids = centroids(unassignedDetections, :);
       bboxes = bboxes(unassignedDetections, :);
.
       for i = 1:size(centroids, 1)
.
           centroid = centroids(i,:);
           bbox = bboxes(i, :);
.
           % Create a Kalman filter object.
           kalmanFilter = configureKalmanFilter('ConstantVelocity', ...
               centroid, [200, 50], [100, 25], 200);
.
           % Create a new track.
           newTrack = struct(...
               'id', nextId, ...
               'bbox', bbox, ...
               'kalmanFilter', kalmanFilter, ...
               'age', 1, ...
               'totalVisibleCount', 4, ...
               'consecutiveInvisibleCount', 0);
.
           % Add it to the array of tracks.
           tracks(end + 1) = newTrack;
.
           % Increment the next id.
           nextId = nextId + 1;
       end
   end
.
   %% Display Tracking Results
   % The |displayTrackingResults| function draws a bounding box and label ID 
   % for each track on the video frame and the foreground mask. It then 
   % displays the frame and the mask in their respective video players. 
.
   function displayTrackingResults()
       % Convert the frame and the mask to uint8 RGB.
       frame = im2uint8(frame);
       mask = uint8(repmat(mask, [1, 1, 3])) .* 255;
.
       minVisibleCount = 8;
       if ~isempty(tracks)
.
           % Noisy detections tend to result in short-lived tracks.
           % Only display tracks that have been visible for more than 
           % a minimum number of frames.
           reliableTrackInds = ...
               [tracks(:).totalVisibleCount] > minVisibleCount;
           reliableTracks = tracks(reliableTrackInds);
.
           % Display the objects. If an object has not been detected
           % in this frame, display its predicted bounding box.
           if ~isempty(reliableTracks)
               % Get bounding boxes.
               bboxes = cat(1, reliableTracks.bbox);
.
               % Get ids.
               ids = int32([reliableTracks(:).id]);
.
               % Create labels for objects indicating the ones for 
               % which we display the predicted rather than the actual 
               % location.
               labels = cellstr(int2str(ids'));
               predictedTrackInds = ...
                   [reliableTracks(:).consecutiveInvisibleCount] > 0;
               isPredicted = cell(size(labels));
               isPredicted(predictedTrackInds) = {' predicted'};
               labels = strcat(labels, isPredicted);
.
               labels = 'Red Ball';
               % Draw the objects on the frame.
               frame = insertObjectAnnotation(frame, 'rectangle', ...
                   bboxes, labels);
.
               % Draw the objects on the mask.
               mask = insertObjectAnnotation(mask, 'rectangle', ...
                   bboxes, labels);
           end
       end
.
       % Display the mask and the frame.      
       obj.videoPlayer.step(frame);
   end
.
   %% Store previous location
   % The |saveposition| function stores the location of the previous frame to
   % allow for the calculation of a direction vector created from consecutive
   % frames
.
   function saveposition()
       if fr <2
          Lastpos = [0 0];
       elseif fr >= 2
          Lastpos = Currentpos;
       end
   end
.
   %% Check for a collision
   % The |checkcollision| function checks every frame if there is a sudden
   % change in the direction compared to its previous. If more elaborate 
   % collision rules apply, one can look to find the acceleration
.
   function checkcollision()
       if ~isempty(Lastpos) && fr >17  %% Bounding box drawn
           if (Currentpos(1) > Lastpos(1))&&(Currentpos(2) > Lastpos(2)) ...
                   && colvar == 1
               fprintf('BOTSING in frame %u \n', fr);
               if SHOW == 1
                   h =msgbox('Collision occured!');
               end
               colvar = colvar + 1;
           end
       end
   end
. 

end

A link to the matlab code can be found here: link title

Personal tools