www.gusucode.com > IPCV_Eval_Kit_R2019a_0ce6858工具箱matlab程序源码 > IPCV_Eval_Kit_R2019a_0ce6858/code/demo_files/I4_08_3_particleFilterExample.m

    %% 弶婜壔
clc;clear;close all;imtool close all; rng('default');

%% 摦夋偺妋擣 (傾僾儕働乕僔儑儞)
%implay('singleball.mp4');

%% 僷乕僥傿僋儖僼傿儖僞偺嶌惢
% We are measurement 6 states here (x, xd, xdd, y, yd, ydd)
pf = robotics.ParticleFilter;
pf.StateTransitionFcn = @I4_08_3_video_state_transition;
pf.MeasurementLikelihoodFcn = @I4_08_3_video_measurement;
pf.ResamplingMethod = 'stratified';
pf.StateEstimationMethod = 'mean';     % 慡棻巕埵抲偺暯嬒偱丄嵟廔悇掕抣傪寛掕

%%  僔僗僥儉僆僽僕僃僋僩嶌惉
videoReader = vision.VideoFileReader('singleball.mp4');
sz = get(0,'ScreenSize');
videoPlayer = vision.VideoPlayer('Position', [180,sz(4)-490,500,400]);
foregroundDetector = vision.ForegroundDetector('NumTrainingFrames', 10, 'InitialVariance', 0.05);
blobAnalyzer = vision.BlobAnalysis('AreaOutputPort', false, 'MinimumBlobArea', 70);

%% 僐儅憲傝儃僞儞昞帵
a=true;
sz = get(0,'ScreenSize');
figure('MenuBar','none','Toolbar','none','Position',[20 sz(4)-100 100 70])
uicontrol('Style', 'pushbutton', 'String', '師偺僼儗乕儉',...
        'Position', [20 20 80 40],'Callback', 'a=false;');

isFilterInitialized = false;
trackedPositions = [0 0 0];
position = [];

%% 摦夋傪堦僼儗乕儉偢偮張棟
%      "師偺僼儗乕儉" 偺儃僞儞偱丄僐儅憲傝
while ~isDone(videoReader)

    frame  = step(videoReader);  % 夋憸傪1僼儗乕儉撉崬傒

    % 儃乕儖 (慜宨)偺専弌丒拞怱揰専弌
    foregroundMask = step(foregroundDetector, rgb2gray(frame));
    detectedLocation = step(blobAnalyzer, foregroundMask);
    isObjectDetected = ~isempty(detectedLocation);
    
    if ~isFilterInitialized   % 僩儔僢僉儞僌巒傑偭偰偄側偄応崌
      if isObjectDetected        % 儃乕儖傪専弌偟偨偲偒
      % 儃乕儖偑嵟弶偵専弌偝傟偨帪丄偦偺埵抲偱棻巕(僷乕僥傿僋儖僼傿儖僞)傪弶婜壔
        numParticles = 8000;
        initialState = [detectedLocation(1) 20 0 detectedLocation(2) 0 0];
        initialCovariance = diag([16^2 4^2 4^2 16^2 4^2 4^2]);     % 嫟暘嶶峴楍
        initialize(pf, numParticles, initialState, initialCovariance);
        
        isFilterInitialized = true;
        trackedLocation = correct(pf, detectedLocation(1,:));
        label = 'Initial';
      else   % 儃乕儖偑傑偩尒偮偐偭偰偄側偄応崌
        trackedLocation = [];label = '';
      end
    else    % 僩儔僢僉儞僌拞偺応崌
      if isObjectDetected  % 儃乕儖偑専弌偝傟偨応崌
        % [梊應]  棻巕悢暘僲僀僘傪惗惉
        %         奺棻巕偺師帪娫偺埵抲傪忬懺慗堏儌僨儖偱梊應偟偨屻丄奺棻巕偵僲僀僘傪壛偊傞
        %         stateTransitionFcn 偑屇偽傟傞
        predict(pf);
        
        % [悇掕] 娤應儌僨儖偵傛傝丄奺梊應埵抲偱偺栟搙傪寁嶼
        %        MeasurementLikelihoodFcn 偑屇偽傟傞:
        %        梊應偟偨奺棻巕埵抲偲丄幚嵺偺娤應抣偺嫍棧偵傛傝栟搙傪寛掕
        %        栟搙偵廬偄丄棻巕傪嵞攝抲乮儕僒儞僾儖乯
        %         乮栟搙偺掅偄埵抲偺棻巕偼徚柵丅栟搙偺崅偄埵抲偵偼暋悢偺棻巕傪攝抲乯
        %        慡棻巕偺暯嬒埵抲偱嵟廔梊應埵抲傪寛掕
        trackedLocation = correct(pf, detectedLocation(1,:));
        label = 'Corrected';
      else  % 僩儔僢僉儞僌拞偵儃乕儖偑尒偮偐傜側偐偭偨応崌
        % 梊應
        trackedLocation = predict(pf);
        label = 'Predicted';
      end
    end
    
  % 摦夋昞帵
  % 専弌偝傟偨偲偒丄専弌偝傟偨埵抲偵惵廫帤儅乕僋
  if isObjectDetected
    combinedImage = insertMarker(frame, detectedLocation, 'Color','blue', 'Size',5);
  else
    combinedImage = frame;
  end
  
  % 僩儔僢僋(専弌or梊應)偝傟偰偄傞偲偒丄応強偵愒娵傪廳偹彂偒
  if ~isempty(trackedLocation)
    % 棻巕埵抲偵丄儅僕僃儞僞怓偺揰
    particleLoc = pf.Particles(:,[1,4]);
    particleLoc(:,3) = 0.7;           % 墌偺敿宎
    combinedImage = insertShape(combinedImage, 'FilledCircle', particleLoc, 'Color', 'magenta');
 
    position = trackedLocation([1,4]);
    position(:, 3) = 5;           % 墌偺敿宎
    combinedImage = insertObjectAnnotation(combinedImage, 'circle', position,label);
    %combinedImage = insertMarker(combinedImage, trackedLocation, 'Color','red');    position, {label}
  
    % 夁嫀偺揰偵椢娵
    combinedImage = insertShape(combinedImage, 'Circle', trackedPositions, 'Color', 'green');
    trackedPositions = [trackedPositions; position];
  end

  step(videoPlayer, combinedImage);   % 價僨僆1僼儗乕儉昞帵

  while (a) 
    drawnow;   % 僾僢僔儏儃僞儞偺僀儀儞僩偺妋擣
  end;
  a = true;

end % while

release(videoReader);

%% 廔椆















%% 儕僒儞僾儕儞僌偺曽朄
% multinomial (default)
% residual
% stratified
% systematic


%%
% Copyright 2016 The MathWorks, Inc.