weighted_nearest_neighbor3D_MP.m

function tracks = weighted_nearest_neighbor3D_MP(p2_est,p1_pos,p2_pos,paras_1,paras_2,s_radius,weights)

% modified based on the multi-parametric PTV by Cardwell's paper
% tracking objects defined by 
% 1) position (x,y,z) that changes over frames
% 2) property parameters (p_1,2,3,...) that remain almost unchanged between
% adjacent frames.
% This function finds the most probable matchings between the tracking
% objects from two frames by looking at the minimum of weighted
% difference calculated from position and property info.

% inputs:
% 1) p2_est, estimated positions of objects in the 2nd frame
% 2) p1_pos,p2_pos, positions of objects in the two frames
% 3) paras_1,paras_2, properties of objects in the two frames
% 4) s_radius, search radius from the estimated position
% 5) weights, relative importance of each property
% outputs:
% 1) tracks, the most probable matching of objects between two frames.

% this code is part of Prana from Vlachos reseach group at Purdue
% University
% by Tianqi Guo. (Jan. 2018)
%==========================================================================

% number of tracking objects from two frames
num_p1 = length(p1_pos);  num_p2 = length(p2_pos);  

% number of property parameters
num_paras = size(paras_1,2);

% maximum difference of each property parameter
paras_diff_max = zeros(1,num_paras);

% calculate the maximum difference for each parameter
for i = 1 : num_paras    
    paras_diff_max(i) = max([paras_1(:,i);paras_2(:,i)]) - min([paras_1(:,i);paras_2(:,i)]);    
end

% if the parameter is the same for all objects, 
% exclude it from the difference calculation
paras_diff_max(paras_diff_max == 0) = Inf;

% record the actual positions
p1_org = p1_pos;
p1_pos = p2_est;

% placeholder
compare = cell(num_p1,1);

% for each object in the 1st frame
% could be replaced with regular for loop
for i = 1:num_p1
    
    % difference in positions\distance between this object and all the
    % objects in the 2nd frame
    dX = p2_pos(:,1) - p1_pos(i,1);  
    dY = p2_pos(:,2) - p1_pos(i,2);  
    dZ = p2_pos(:,3) - p1_pos(i,3);    
    distance = sqrt(dX.^2+dY.^2+dZ.^2);
    
    % only consider the objects within the search radius
    p_index = find(distance<=s_radius);  
    compare_i = zeros(length(p_index),1);
    % structure of compare_i 
    % [object # in 1st frame,object # in 2nd frame,weighted difference]

    % This statement catches the possibility that no paritlces are with in
    % the search radius.
    if ~isempty(p_index)
        
        % object # in 1st frame
        compare_i(:,1) = ones(size(p_index)).*i;
        
        % object # in 2nd frame
        compare_i(:,2) = p_index;
        
        % matching probablility\weighted difference
        Prob = zeros(size(p_index,1),1+num_paras);
        
        % compute the match probability\weighted difference for each possible pairing
        Prob(:,1) = (distance(p_index)./s_radius).*weights(1);
        % for each tracking parameter
        for j = 1:num_paras
            
            % normalized by the maximum difference of this parameter
            Prob(:,j+1) = (abs(paras_2(p_index,j) - paras_1(i,j))./paras_diff_max(j)).*weights(j+1);
            
        end
        
        % the weighted difference
        compare_i(:,3)=sum(Prob,2)./sum(weights);
        
        % populate main 'compare' array
        compare{i}=compare_i;
    end

%     clear dX dY dZ distance p_index compare_i Prob
end
clear i
compare = cell2mat(compare);

% Check to make sure that the compare variable is not empty.  It will be
% empty if no particles where paired durring this processes.  The use
% should try increasing there search radius.
if ~isempty(compare)
    %sort the comparison array in ascending order based on the pairing
    %probability
    compare_sort=sortrows(compare,3);
    
    %determine the best particle matches and successively match particles
    %until the max number of pairs has been reached (or no more exist)
    p_pairs=[];
    max_matches=min(num_p1,num_p2);
    found_matches_im1=[];  found_matches_im2=[];
    c=0;  i=1;
    while (c<=max_matches) && (i<=size(compare_sort,1))
        test1=found_matches_im1==compare_sort(i,1);
        test2=found_matches_im2==compare_sort(i,2);
        if (any(test1) || any(test2))
            i=i+1;
        else
            found_matches_im1=vertcat(found_matches_im1,compare_sort(i,1));
            found_matches_im2=vertcat(found_matches_im2,compare_sort(i,2));
            p_pairs=vertcat(p_pairs,compare_sort(i,:));
            i=i+1;
            c=c+1;
        end
    end
    
    %populate the 'tracks' arrays with the following structure:
    %   tracks=[X1, X2, Y1, Y2, Z1, Z2, p#1 p#2 match_probability]
    tracks = [p_pairs(:,:), p1_org(p_pairs(:,1),:), p2_pos(p_pairs(:,2),:)];
else
    tracks = [];
end