function out_ballgt = do_balltrack( ppm_dir, ballgt, verbose ) % PPM reading functions courtesy of Dr Jean-Marc Odobez - odobez@idiap.ch if nargin < 2 error( [ mfilename ' needs at least 2 input arguments.' ] ); end if ~exist( 'verbose', 'var' ) verbose = 0; end x_radius_1 = 1e-3; y_radius_1 = 1e-3; x_radius_2 = 1e-3; y_radius_2 = 1e-3; % Interpolate out_ballgt = ballgt; if verbose disp( sprintf( [ mfilename ': maximum frame index:%d' ], max( out_ballgt( :, 1 ) ) ) ); chrono = chrono_start( max( out_ballgt( :, 1 ) ), 30 ); end; last_processed = 0; while 1 % is_visible ind = find( out_ballgt( last_processed+1:end, 3 ) ); if isempty( ind ), break, end; first_visible = last_processed + ind( 1 ); % is_visible ind = find( out_ballgt( first_visible+1:end, 3 ) ); if ~isempty( ind ) next_visible = first_visible + ind( 1 ); % Check the ball is continuously visible between the two frames ind = first_visible+1:next_visible-1; do_track = 1; if ~isempty( ind ) if any( out_ballgt( ind, 2 ) & ~out_ballgt( ind, 3 ) ) % If not always visible, then skip the whole interval last_processed = next_visible; do_track = 0; end end % Let's do tracking if we can if do_track first_frame = out_ballgt( first_visible, 1 ); last_frame = out_ballgt( next_visible, 1 ); if last_frame - first_frame > 1 % First create a model for frames "first_frame" and "last_frame" if ~exist( 'model', 'var' ) | ( length( model ) < first_frame ) | isempty( model( first_frame ).color_model ) if verbose; disp( sprintf( [ mfilename ': building model for frame %d' ], first_frame ) ); end; model( first_frame ) = build_model( ppm_dir, first_frame, out_ballgt( first_visible, 4:7 ) ); x1x2 = model( first_frame ).xybox( [ 1 3 ] ); x_radius_1 = abs( diff( x1x2 ) ) / 2; y1y2 = model( first_frame ).xybox( [ 2 4 ] ); y_radius_1 = abs( diff( y1y2 ) ) / 2; end if ~exist( 'model', 'var' ) | ( length( model ) < last_frame ) | isempty( model( last_frame ).color_model ) if verbose; disp( sprintf( [ mfilename ': building model for frame %d' ], last_frame ) ); end; model( last_frame ) = build_model( ppm_dir, last_frame, out_ballgt( next_visible, 4:7 ) ); x1x2 = model( last_frame ).xybox( [ 1 3 ] ); x_radius_2 = abs( diff( x1x2 ) ) / 2; y1y2 = model( last_frame ).xybox( [ 2 4 ] ); y_radius_2 = abs( diff( y1y2 ) ) / 2; end % Second, interpolate between these two frames by tracking the ball % Pick the largest color model n1 = length( model( first_frame ).color_model.ind_pix_3d ); n2 = length( model( last_frame ).color_model.ind_pix_3d ); if n1 < n2 model( first_frame ).color_model = model( last_frame ).color_model; elseif n1 > n2 model( last_frame ).color_model = model( first_frame ).color_model; end % Constraint on the radius % This improves results a bit x_radius_min = min( x_radius_1, x_radius_2 ); y_radius_min = min( y_radius_1, y_radius_2 ); % ( 1 ) estimation for frame "first_frame+1" if verbose; disp( sprintf( [ mfilename ': creating GT for frame %d...' ], first_frame+1 ) ); end; % Initialization: linear interpolation on xybox alpha = 1 - 1 / ( last_frame - first_frame ); model( first_frame + 1 ).xybox = alpha * model( first_frame ).xybox + ( 1-alpha ) * model( last_frame ).xybox; % Simply copy the color model model( first_frame + 1 ).color_model = model( first_frame ).color_model; % Update the location of the box (size is untouched) model( first_frame + 1 ) = update_model( ppm_dir, first_frame + 1, model( first_frame + 1 ), x_radius_min, y_radius_min ); % Store it out_ballgt = [ out_ballgt; first_frame + 1, 0, 1, model( first_frame + 1 ).xybox ]; % ( 2 ) estimation for frame "last_frame-1" (if different frame) if last_frame-1 > first_frame+1 if verbose; disp( sprintf( [ mfilename ': creating GT for frame %d...' ], last_frame-1 ) ); end; % Initialization: linear interpolation on xybox alpha = 1 / ( last_frame - first_frame ); model( last_frame - 1 ).xybox = alpha * model( first_frame ).xybox + ( 1-alpha ) * model( last_frame ).xybox; % Simply copy the color model model( last_frame - 1 ).color_model = model( last_frame ).color_model; % Update the location of the box (size is untouched) model( last_frame - 1 ) = update_model( ppm_dir, last_frame - 1, model( last_frame - 1 ), x_radius_min, y_radius_min ); % Store it out_ballgt = [ out_ballgt; last_frame - 1, 0, 1, model( last_frame - 1 ).xybox ]; end % Sort the new GT matrix [ tmp, ind ] = sort( out_ballgt( :, 1 ) ); out_ballgt = out_ballgt( ind, : ); end end end % Mark it as processed last_processed = first_visible; if verbose; chrono = chrono_check( chrono, out_ballgt( last_processed, 1 ) ); end; end % At least make sure we keep original measurements ind_new = find( ~ismember( out_ballgt(:,1), ballgt(:,1) ) ); out_ballgt = sortrows( [ ballgt; out_ballgt( ind_new, : ) ] ); if verbose; disp( [ mfilename ': done.' ] ); end; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function model = build_model( ppm_dir, frame, xybox ) if nargin < 3 error( [ mfilename ':build_model() needs 3 input arguments.' ] ); end ppm_name = fullfile( ppm_dir, sprintf( 'image%05d.ppm', frame ) ); im = rgb2rgbn( Lecture( ppm_name ) ); nrows = size( im, 1 ); ncols = size( im, 2 ); nlayers = size( im, 3 ); % Convert xybox into an ellipse ellipse = xybox2ellipse( xybox ); % Determine which pixels are inside the ellipse r = repmat( (1:nrows).', 1, ncols ); c = repmat( (1:ncols), nrows, 1 ); is_inside = ( ( ( ( r - ellipse.y_center ) / ellipse.y_radius ) .^ 2 + ( ( c - ellipse.x_center ) / ellipse.x_radius ) .^ 2 ) < 1+eps ); % (row,col) coordinates in the image "im" [ u,v ] = find( is_inside ); % (row,col) coordinates in the color model = subimage within "im" u2 = u-min(u)+1; v2 = v-min(v)+1; % Copy them into "color_model.pixels" color_model.pixels = repmat( NaN, [ max( u2 ), max( v2 ), nlayers ] ); for a = 1:nlayers ind_orig = sub2ind( size( im ), u(:), v(:), repmat( a, size( u(:) ) ) ); ind_copy = sub2ind( size( color_model.pixels ), u2(:), v2(:), repmat( a, size( u2(:) ) ) ); color_model.pixels( ind_copy ) = im( ind_orig ); end % Note where the center of the ellipse is, in "color_model.pixels" color_model.x_center = 1 + ellipse.x_center - min( v ); color_model.y_center = 1 + ellipse.y_center - min( u ); % List of the pixels % ( 1 ) all layers together ind_pix_3d = find( ~isnan( color_model.pixels ) ); color_model.ind_pix_3d = ind_pix_3d; % ( 2 ) in 2D ind_pix_2d = find( ~isnan( color_model.pixels( :,:,1 ) ) ); color_model.ind_pix_2d = ind_pix_2d; [ r_pix, c_pix ] = find( ~isnan( color_model.pixels( :,:,1 ) ) ); color_model.r_pix = r_pix; color_model.c_pix = c_pix; % Also store their coordinates relative to the center, % and the radii of the ellipse, for scaling color_model.x_pix = c_pix - color_model.x_center; color_model.y_pix = r_pix - color_model.y_center; color_model.x_radius = ellipse.x_radius; color_model.y_radius = ellipse.y_radius; % Store the whole thing (xybox + color model) and return model.xybox = xybox; model.color_model = color_model; if 0 % DEBUG figure; imagesc( uint8( color_model.pixels ) ); keyboard end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function new_model = update_model( ppm_dir, frame, model, x_radius_min, y_radius_min ) if nargin < 3 error( [ mfilename ':update_model() needs at least 3 input arguments.' ] ); end if ~exist( 'x_radius_min', 'var' ) x_radius_min = 1e-3; end if ~exist( 'y_radius_min', 'var' ) y_radius_min = 1e-3; end % For convergence tests PIXEL_THRESHOLD = 1e-3; MAX_ITER = 10; % For cropping the image CROP = [ 5 7 355 287 ]; % x_UPLEFT y_UPLEFT x_LOWRIGHT y_LOWRIGHT % For 2D interpolation INTERP2_METHOD = 'linear'; % For new location propositions factor = [ -4 -3 -2 -1.5 -1 -0.75 -0.5 -0.25 -0.05 -0.01 ]; factor = [ factor, 0, fliplr( -factor ) ]; dx = factor; dy = factor; % dxy = combvec( dx, dy ); dxy = peigne_vec( dx, dy ); ind = find( sum( dxy .^ 2 ) < max( factor .^ 2 ) + eps ); dxy = dxy( :, ind ); ellipse = xybox2ellipse( model.xybox ); prop.dxy( 1,: ) = dxy( 1,: ) * ellipse.x_radius; prop.dxy( 2,: ) = dxy( 2,: ) * ellipse.y_radius; clear dxy; n = size( prop.dxy, 2 ); % For trying new radius propositions rfactor = [ 0.9 0.95 0.98 0.99 1 1.01 1.02 1.05 1.1 ]; % prop.rxry = combvec( rfactor, rfactor ); prop.rxry = peigne_vec( rfactor, rfactor ); nr = size( prop.rxry, 2 ); % new_model = model; % Load PPM image ppm_name = fullfile( ppm_dir, sprintf( 'image%05d.ppm', frame ) ); im = rgb2rgbn( Lecture( ppm_name ) ); % Crop it im = im( CROP(2):CROP(4), CROP(1):CROP(3), : ); a = - CROP( 1 ) + 1; b = - CROP( 2 ) + 1; new_model.xybox = new_model.xybox + [ a b a b ]; % Dimensions nrows = size( im, 1 ); ncols = size( im, 2 ); nlayers = size( im, 3 ); % Start iterating ind_pix_2d = new_model.color_model.ind_pix_2d; ind_pix_3d = new_model.color_model.ind_pix_3d; x_pix = new_model.color_model.x_pix; y_pix = new_model.color_model.y_pix; npix = length( x_pix ); pix = new_model.color_model.pixels( ind_pix_3d(:) ); % tmp = diff( new_model.color_model.pixels, [], 2 ); tmp2 = repmat( NaN, size( new_model.color_model.pixels ) ); tmp2( :, 1:end-1, : ) = tmp; tmp3 = repmat( NaN, size( new_model.color_model.pixels ) ); tmp3( :, 2:end, : ) = tmp; pix_dx_ind = [ find( ~isnan( tmp2(:) ) ), find( ~isnan( tmp3(:) ) ) ]; for a = 1:length( pix_dx_ind(:) ) ind = find( ind_pix_3d == pix_dx_ind( a ) ); pix_dx_ind( a ) = ind; end pix_dx = pix( pix_dx_ind( :,1 ) ) - pix( pix_dx_ind( :,2 ) ); % tmp = diff( new_model.color_model.pixels, [], 1 ); tmp2 = repmat( NaN, size( new_model.color_model.pixels ) ); tmp2( 1:end-1,:,: ) = tmp; tmp3 = repmat( NaN, size( new_model.color_model.pixels ) ); tmp3( 2:end,:,: ) = tmp; pix_dy_ind = [ find( ~isnan( tmp2(:) ) ), find( ~isnan( tmp3(:) ) ) ]; for a = 1:length( pix_dy_ind(:) ) ind = find( ind_pix_3d == pix_dy_ind( a ) ); pix_dy_ind( a ) = ind; end pix_dy = pix( pix_dy_ind( :,1 ) ) - pix( pix_dy_ind( :,2 ) ); % Move the xybox to obtain the best color fit % = smallest color distance iter = 0; while 1 prev_model = new_model; % ( 1 ) Optimize location (fixed radius) % Create candidates cand.xybox = repmat( new_model.xybox, n, 1 ); cand.xybox( :, [ 1 3 ] ) = cand.xybox( :, [ 1 3 ] ) + repmat( prop.dxy( 1, : ).', 1, 2 ); cand.xybox( :, [ 2 4 ] ) = cand.xybox( :, [ 2 4 ] ) + repmat( prop.dxy( 2, : ).', 1, 2 ); % Keep only candidate boxes entierly within the boundaries of the image ind = find( ( cand.xybox( :,1 ) > 0.5 ) & ( cand.xybox( :,3 ) < ncols + 0.5 ) & ... ( cand.xybox( :,2 ) > 0.5 ) & ( cand.xybox( :,4 ) < nrows + 0.5 ) ); m = length( ind ); if m < 1, break, end; cand.xybox = cand.xybox( ind, : ); % Start processing the candidates cand.x_center = mean( cand.xybox( :,[1 3] ), 2 ); cand.y_center = mean( cand.xybox( :,[2 4] ), 2 ); cand.x_radius = abs( diff( cand.xybox( :,[1 3] ), [], 2 ) ) / 2; cand.y_radius = abs( diff( cand.xybox( :,[2 4] ), [], 2 ) ) / 2; cand.x_factor = cand.x_radius / new_model.color_model.x_radius; cand.y_factor = cand.y_radius / new_model.color_model.y_radius; % Coordinates of the pixels, for each candidate (NOT integers) cand.x_pix_mat = repmat( cand.x_center(:).', npix, 1 ) + x_pix(:) * cand.x_factor(:).'; cand.y_pix_mat = repmat( cand.y_center(:).', npix, 1 ) + y_pix(:) * cand.y_factor(:).'; % Extract colors at those coordinates reality = repmat( NaN, npix * nlayers, m ); for a = 1:nlayers reality( npix * (a-1) + (1:npix).', : ) = interp2( im( :,:,a ), cand.x_pix_mat, cand.y_pix_mat, INTERP2_METHOD ); end % Extract delta features reality_dx = reality( pix_dx_ind( :,1 ), : ) - reality( pix_dx_ind( :, 2 ), : ); reality_dy = reality( pix_dy_ind( :,1 ), : ) - reality( pix_dy_ind( :, 2 ), : ); % Compare with the color model color_dist = sum( ( reality - repmat( pix, 1, m ) ) .^ 2 ); color_dx_dist = sum( ( reality_dx - repmat( pix_dx, 1, m ) ) .^ 2 ); color_dy_dist = sum( ( reality_dy - repmat( pix_dy, 1, m ) ) .^ 2 ); % Pick the closest hypothesis criterion = dist2criterion( color_dist, color_dx_dist, color_dy_dist ); [ tmp, ind_min ] = min( criterion ); new_model.xybox = cand.xybox( ind_min, : ); % ( 2 ) Optimize radii (fixed center) clear cand; % Create the candidates ellipse = xybox2ellipse( new_model.xybox ); tmp_x = prop.rxry( 1,: ) * ellipse.x_radius; tmp_y = prop.rxry( 2,: ) * ellipse.y_radius; % Keep only those candidates that are big enough % This trick is NOT vital for stability, but it improves the quality of the result ind = find( ( tmp_x >= x_radius_min ) & ( tmp_y >= y_radius_min ) ); if ~isempty( ind ) cand.x_factor = tmp_x( ind ) / new_model.color_model.x_radius; cand.y_factor = tmp_y( ind ) / new_model.color_model.y_radius; % Coordinates of the pixels, for each candidate (NOT integers) cand.x_pix_mat = ellipse.x_center + x_pix(:) * cand.x_factor(:).'; cand.y_pix_mat = ellipse.y_center + y_pix(:) * cand.y_factor(:).'; % Extract colors at those coordinates m = length( cand.x_factor ); reality = repmat( NaN, npix * nlayers, m ); for a = 1:nlayers reality( npix * (a-1) + (1:npix).', : ) = interp2( im( :,:,a ), cand.x_pix_mat, cand.y_pix_mat, INTERP2_METHOD ); end % Extract delta features reality_dx = reality( pix_dx_ind( :,1 ), : ) - reality( pix_dx_ind( :, 2 ), : ); reality_dy = reality( pix_dy_ind( :,1 ), : ) - reality( pix_dy_ind( :, 2 ), : ); % Compare with the color model color_dist = sum( ( reality - repmat( pix, 1, m ) ) .^ 2 ); color_dx_dist = sum( ( reality_dx - repmat( pix_dx, 1, m ) ) .^ 2 ); color_dy_dist = sum( ( reality_dy - repmat( pix_dy, 1, m ) ) .^ 2 ); % Pick the closest hypothesis criterion = dist2criterion( color_dist, color_dx_dist, color_dy_dist ); [ tmp, ind_min ] = min( criterion ); ellipse.x_radius = ellipse.x_radius * prop.rxry( 1, ind_min ); ellipse.y_radius = ellipse.x_radius * prop.rxry( 2, ind_min ); new_model.xybox = ellipse2xybox( ellipse ); end % ( 3 ) Termination test iter = iter+1; if iter >= MAX_ITER, break, end; if max( abs( prev_model.xybox - new_model.xybox ) ) < PIXEL_THRESHOLD, break, end; end % Move back to origin a = CROP( 1 ) - 1; b = CROP( 2 ) - 1; new_model.xybox = new_model.xybox + [ a b a b ]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function ellipse = xybox2ellipse( xybox ) if nargin < 1 error( [ mfilename ':ellipse() needs one input argument.' ] ); end x1x2 = xybox( [ 1 3 ] ); y1y2 = xybox( [ 2 4 ] ); ellipse.x_center = mean( x1x2 ); ellipse.y_center = mean( y1y2 ); ellipse.x_radius = abs( diff( x1x2 ) ) / 2; ellipse.y_radius = abs( diff( y1y2 ) ) / 2; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function xybox = ellipse2xybox( ellipse ) if nargin < 1 error( [ mfilename ':xybox2ellipse() needs one input argument.' ] ); end xybox = [ ellipse.x_center - ellipse.x_radius, ... ellipse.y_center - ellipse.y_radius, ... ellipse.x_center + ellipse.x_radius, ... ellipse.y_center + ellipse.y_radius ]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function rgbn_im = rgb2rgbn( rgb_im ) if nargin < 1 error( [ mfilename ':rgb2rg() needs one input argument.' ] ); end a_sum = sum( rgb_im, 3 ); a_sum( find( ~a_sum ) ) = eps; rgbn_im = rgb_im ./ repmat( a_sum, [ 1 1 size( rgb_im, 3 ) ] ); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function criterion = dist2criterion( color_dist, color_dx_dist, color_dy_dist ) if nargin < 3 error( [ mfilename ':dist2criterion() needs 3 input arguments.' ] ); end criterion = color_dist + ( color_dx_dist + color_dy_dist ) / 2; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function y = peigne_vec( varargin) if length(varargin) == 0 y = []; else y = varargin{1}; for i=2:length(varargin) z = varargin{i}; y = [copy_blocked(y,size(z,2)); copy_interleaved(z,size(y,2))]; end end %========================================================= function b = copy_blocked(m,n) [mr,mc] = size(m); b = zeros(mr,mc*n); ind = 1:mc; for i=[0:(n-1)]*mc b(:,ind+i) = m; end %========================================================= function b = copy_interleaved(m,n) [mr,mc] = size(m); b = zeros(mr*n,mc); ind = 1:mr; for i=[0:(n-1)]*mr b(ind+i,:) = m; end b = reshape(b,mr,n*mc); % ---------------------------------------------------------------------- function Image=Lecture(nom) % Image=Lecture(nom) : Lecture d'une image % format supporté : pgm et ceux de imread (jpeg, tiff, ...) % exemple : Im1=Lecture('lezard.pgm'); % % Pour une image brute, voir LectureRaw [s,r]=strtok(nom,'.'); while ~size(r,2)==0 [s,r]=strtok(r,'.'); end if strcmp(s,'pgm')==1 Image=double(LecturePgm(nom)); elseif strcmp(s,'ppm')==1 Image=double(LecturePnm(nom)); elseif strcmp(s,'pnm')==1 Image=double(LecturePnm(nom)); else Image=imread(nom); Image=double(Image); end % ---------------------------------------------------------------------- function Image=LecturePgm(nom) % Image=LecturePgm(nom) : Lecture d'une image au format pgm % exemple : Im1=LecturePgm('lezard.pgm'); % s=size(nom); s=s(2); while nom(s)==' ' s=s-1; end fid=fopen(nom(1:s),'r'); nl=abs(10); Ent=fscanf(fid,'%c',2); if Ent=='P5' c=fscanf(fid,'%c',1); while c~=nl, c=fscanf(fid,'%c',1); end c=fscanf(fid,'%c',1); while c=='#', while c~=nl, c=fscanf(fid,'%c',1); end c=fscanf(fid,'%c',1); end else 'Ce n''est pas un fichier pgm' end %Traitement des nombres de colonnes et de lignes dim=c; c=fscanf(fid,'%c',1); while c~=nl, dim=[dim,c]; c=fscanf(fid,'%c',1); end ndim=length(dim); nb=strtok(dim,' '); dim=dim(length(nb)+1:ndim); Nbco=str2num(nb); Nbli=str2num(dim); %On trouve 255 puis, ligne suivante, les donnees s=fscanf(fid,'%c',3); if s~='255' 'Attention, erreur de format' end c=fscanf(fid,'%c',1); while c~=nl, c=fscanf(fid,'%c',1); end %%% Recuperation des donnees dans une matrice Image=fread(fid,[Nbco,Nbli]); Image=Image'; fclose(fid); % ---------------------------------------------------------------------- function Image=LecturePnm(nom) % Image=LecturePnm(nom) : Lecture d'une image au format pnm % exemple : Im1=LecturePnm('lezard.pnm'); % s=size(nom); s=s(2); while nom(s)==' ' s=s-1; end sname=s; %stupid but works for p=0:2 fid=fopen(nom(1:sname),'r'); nl=abs(10); Ent=fscanf(fid,'%c',2); if Ent=='P6' c=fscanf(fid,'%c',1); while c~=nl, c=fscanf(fid,'%c',1); end c=fscanf(fid,'%c',1); while c=='#', while c~=nl, c=fscanf(fid,'%c',1); end c=fscanf(fid,'%c',1); end else 'Ce n''est pas un fichier ppm' end %Traitement des nombres de colonnes et de lignes dim=c; c=fscanf(fid,'%c',1); while c~=nl, dim=[dim,c]; c=fscanf(fid,'%c',1); end ndim=length(dim); nb=strtok(dim,' '); dim=dim(length(nb)+1:ndim); Nbco=str2num(nb); Nbli=str2num(dim); %On trouve 255 puis, ligne suivante, les donnees s=fscanf(fid,'%c',3); if s~='255' 'Attention, erreur de format' end c=fscanf(fid,'%c',1); while c~=nl, c=fscanf(fid,'%c',1); end %%% Recuperation des donnees dans une matrice if p==0 Image=zeros(Nbli,Nbco,3); end if p==1 sss=fread(fid,1,'uchar'); end if p==2 sss=fread(fid,2,'uchar'); end ImageR=fread(fid,[Nbco,Nbli],'uchar',2); Image(:,:,p+1)=ImageR'; fclose(fid); end % end loop on plans