Accelerate Pedestrian Detection with SIMD Code

function detectAndTrack(CAMVideoURL)
%#codegen

% Create an ACF object detector for code generation.
model = coder.load('model.mat');
detector = acfObjectDetector(model.Classifier,model.TrainingOptions);

% Create a multi-object tracker to track multiple objects with Kalman filters. 
[tracker, positionSelector] = setupTracker;


% Set-up the video reader and player.
videoFReader   = vision.VideoFileReader(CAMVideoURL);
videoFWriter   = vision.VideoFileWriter('CamVid_0016E5_result.avi', 'FrameRate', 25);
depVideoPlayer = vision.DeployableVideoPlayer;
sampleFrame = videoFReader();
frameSize   = [size(sampleFrame,1) size(sampleFrame,2)];

% Generate the sample detection from the sample frame.
detectionThresh = 50;
detections = detectObjects(detector, sampleFrame, 0, detectionThresh);
costMatrix = zeros([0 numel(detections)]);
[confirmedTracks,~,allTracks] = updateTracks(tracker, detections, 0, costMatrix);
coder.varsize('allTracks');

currentStep = 0;
totalTime = 0;
while ~isDone(videoFReader)
    % Update frame counters.
    currentStep = currentStep + 1;

    % Read the next frame.
    frame = videoFReader();
    
    tic;

    % Detect the pedestrian every 2 frames.
    detections = {};
    if rem(currentStep,2)
        detections = detectObjects(detector, frame, currentStep, detectionThresh);
    end

    % Track the pedestrian every frame.
    costMatrix = detectionToTrackCost(allTracks, detections, positionSelector, tracker.AssignmentThreshold);
    [confirmedTracks,~,allTracks] = updateTracks(tracker, detections, currentStep, costMatrix);

    % Remove the tracks for people that are far away.
    confirmedTracks = removeNoisyTracks(confirmedTracks, positionSelector, frameSize);
    time = toc;
    totalTime = totalTime+time;
    
    % Insert tracking and frame rate annotations.
    framerate = currentStep/totalTime;
    frame = insertTrackBoxes(frame, confirmedTracks, positionSelector);
    frame = insertText(frame, [20 20], framerate, 'AnchorPoint', 'LeftBottom');

    % Write to the output file and display the annotated frame.
    videoFWriter(frame);
    depVideoPlayer(frame);
end

release(videoFWriter);

    

function [tracker, positionSelector] = setupTracker()
tracker = multiObjectTracker('FilterInitializationFcn', @initBboxFilter, ...
    'AssignmentThreshold', 1, ...
    'NumCoastingUpdates', 5, ... 
    'ConfirmationThreshold', [1 5], ...
    'MaxNumTracks', 10, ...
    'HasCostMatrixInput', true);



% Create a matrix that specifies which state vector elements are the position.
positionSelector = [1 0 0 0 0 0 0 0; ...
                    0 0 1 0 0 0 0 0; ...
                    0 0 0 0 1 0 0 0; ...
                    0 0 0 0 0 0 1 0]; 
end

function filter = initBboxFilter(Detection)
% Step 1: Define the motion model and state.
%   Use a constant velocity model for a bounding box on the image.
%   The state is [x; vx; y; vy; w; wv; h; hv]
%   The state transition matrix is: 
%       [1 dt 0  0 0  0 0  0;
%        0  1 0  0 0  0 0  0; 
%        0  0 1 dt 0  0 0  0; 
%        0  0 0  1 0  0 0  0; 
%        0  0 0  0 1 dt 0  0; 
%        0  0 0  0 0  1 0  0;
%        0  0 0  0 0  0 1 dt; 
%        0  0 0  0 0  0 0  1]
%   Assume dt = 1. This example does not consider a time-variant transition
%   model for the linear Kalman filter.
    dt = 1;
    cvel =[1 dt; 0 1];
    A = blkdiag(cvel, cvel, cvel, cvel);
 
% Step 2: Define the process noise. 
%   The process noise represents the parts of the process that the model
%   does not take into account. For example, in a constant velocity model,
%   the acceleration is neglected.
    G1d = [dt^2/2; dt];
    Q1d = G1d*G1d';
    Q = blkdiag(Q1d, Q1d, Q1d, Q1d);
 
% Step 3: Define the measurement model.
%   Only the position ([x;y;w;h]) is measured.
%   The measurement model is
    H = [1 0 0 0 0 0 0 0; ...
         0 0 1 0 0 0 0 0; ...
         0 0 0 0 1 0 0 0; ...
         0 0 0 0 0 0 1 0];
 
% Step 4: Map the sensor measurements to an initial state vector.
%   Because there is no measurement of the velocity, the v components are
%   initialized to 0:
    state = [Detection.Measurement(1); ...
             0; ...
             Detection.Measurement(2); ...
             0; ...
             Detection.Measurement(3); ...
             0; ...
             Detection.Measurement(4); ...
             0];
 
% Step 5: Map the sensor measurement noise to a state covariance.
%   For the parts of the state that the sensor measures directly, use the
%   corresponding measurement noise components. For the parts that the
%   sensor does not measure, assume a large initial state covariance. That way,
%   future detections can be assigned to the track.
    L = 100; % Large value
    stateCov = diag([Detection.MeasurementNoise(1,1), ...
                     L, ...
                     Detection.MeasurementNoise(2,2), ...
                     L, ...
                     Detection.MeasurementNoise(3,3), ...
                     L, ...
                     Detection.MeasurementNoise(4,4), ...
                     L]);
 
% Step 6: Create the correct filter.
%   In this example, all the models are linear, so use trackingKF as the
%   tracking filter.
    filter = trackingKF(...
        'StateTransitionModel', A, ...
        'MeasurementModel', H, ...
        'State', state, ...
        'StateCovariance', stateCov, ... 
        'MeasurementNoise', Detection.MeasurementNoise, ...
        'ProcessNoise', Q);
end

function detections = detectObjects(detector, frame, frameCount, thresh)
    % Run the detector and return a list of bounding boxes: [x, y, w, h]
    [bboxes_raw, scores] = detect(detector, frame);

    numDetections = sum(scores>thresh);
    bboxes = bboxes_raw(scores>thresh, :);
    
    % Define the measurement noise.
    L = 100;
    measurementNoise = [L 0  0  0; ...
                        0 L  0  0; ...
                        0 0 L/2 0; ...
                        0 0  0 L/2];
                    
    % Formulate the detections as a list of objectDetection reports.
    detections = cell(numDetections, 1);                      
    objAttr = struct('VisionID', uint32(0), 'Size', zeros(1,3,'single'), ...
        'RadarID', uint16(0), 'Status', uint8(0), 'Amplitude', single(0), ...
        'RangeMode', uint8(0));
    
    for i = 1:numDetections
        detections{i} = objectDetection(frameCount, bboxes(i, :), ...
            'MeasurementNoise', measurementNoise, 'ObjectAttributes', {objAttr});
    end
end

function tracks = removeNoisyTracks(tracks, positionSelector, imageSize)

    if isempty(tracks)
        return
    end
    
    % Extract the positions from all the tracks.
    positions = getTrackPositions(tracks, positionSelector);
    % The track is 'invalid' if the predicted position is about to move out
    % of the image, or the bounding box is too small.
    invalid = ( positions(:, 1) < 1 | ...
                positions(:, 1) + positions(:, 3) > imageSize(2) | ...
                positions(:, 3) <= 5 | ...
                positions(:, 4) <= 10 );
    tracks(invalid) = [];
end

function I = insertTrackBoxes(I, tracks, positionSelector)
    if isempty(tracks)
        return
    end

    bboxes = getTrackPositions(tracks, positionSelector);
    for i=1:size(bboxes,1)
        box = bboxes(i, :);
        I = insertShape(I,'Rectangle',int32(box));
    end
end