%This software implements the point matching algorithm described in the paper:
%"Rotation Invariant Nonrigid Point Set Matching in Cluttered Scenes", IEEE transaction on Image Processing, 2012

%Author: Wei Lian (Changzhi University, Changzhi, 046011, Shanxi Province, P.R. China, Email: lianwei3 at gmail.com)

%If you use this software for research purposes, you should cite the aforementioned paper in any resulting publications.


function cost=SC_cost_fast_fun(edges,X,Y,Nang,outlier_resist)
%%generic function for computing oriented SC distance between all possible edges in
%%Y and edges in X
%%edges: Ne x 2, list of triangle edges for X

%%%%parameters
nbins_theta=12;
nbins_r=5;
r_inner=1/8;
r_outer=2;
%%%%%%%%%%%%%%%%%
Ny=size(Y,1);
Ne=size(edges,1);

%%orientation for vectors in Y, heads corresponds to the 1st dimension, tails 2nd dimension
dify=repmat(permute(Y,[1,3,2]),[1,Ny,1])-repmat(permute(Y,[3,1,2]),[Ny,1,1]);%% Ny x Ny x 2, head 1st dim, tail 2nd dim
angy=atan2(dify(:,:,2),dify(:,:,1)); %% Ny x Ny
%%orientation for edges in X
difx=X(edges(:,1),:)-X(edges(:,2),:); %%Ne x 2, head 1st dim, tail 2nd dim
angx=atan2(difx(:,2),difx(:,1)); %% Ne x 1, pointed from 1st edges to 2nd edges

%%mean dist
r_array=real(sqrt(dist2(X',X')));
mean_dist_1=mean(r_array(:));

%%shape context for edges' head points in X
[BHx1,mean_dist_2]=sc_compute(X(edges(:,1),:)',angx',mean_dist_1,nbins_theta,nbins_r,r_inner,r_outer,zeros(1,Ne));
% %%shape context for edges' tail points in X
[BHx2,mean_dist_2]=sc_compute(X(edges(:,2),:)',angx'+pi,mean_dist_1,nbins_theta,nbins_r,r_inner,r_outer,zeros(1,Ne));


cost1=zeros(Ne,Ny,Nang);
cost2=zeros(Ne,Ny,Nang);
for ii=1:Nang
    %%shape context for Y
    [BHy,mean_dist_2]=sc_compute(Y',(ii-1)*2*pi/Nang*ones(1,Ny),mean_dist_1,nbins_theta,nbins_r,r_inner,r_outer,zeros(1,Ny));
    if outlier_resist
        cost1(:,:,ii)=hist_cost_2_mask(BHx1,BHy);%%outlier robust implementation
        cost2(:,:,ii)=hist_cost_2_mask(BHx2,BHy);
    else
        cost1(:,:,ii)=hist_cost_2(BHx1,BHy);
        cost2(:,:,ii)=hist_cost_2(BHx2,BHy);
    end
end

%%quantification for angy
angy=rem(rem(angy,2*pi)+2*pi,2*pi);
angy=1+floor(angy*Nang/(2*pi));

cost_1=zeros(Ne,Ny,Ny);
cost_2=zeros(Ne,Ny,Ny);


for ii=1:Ne
    for jj=1:Ny
        cost_1(ii,jj,:)=cost1(ii,jj,angy(jj,:));
        cost_2(ii,jj,:)=cost2(ii,jj,angy(jj,:));        
    end
end
%%%%%

cost1=permute(cost_1,[2,3,1]);
cost2=permute(cost_2,[3,2,1]);

cost=zeros(Ny,Ny,Ne);
% cost=cost1+cost2;% it's empirically shown to be a bad choice
for ii=1:Ne-1
    if edges(ii,1)~=edges(ii+1,1)%%head is different, so preserving SC for heads
        cost(:,:,ii)=cost1(:,:,ii);
    else
        cost(:,:,ii)=cost2(:,:,ii);
    end
end

if edges(Ne-1,1)~=edges(Ne,1)%%head is different, so preserving SC for heads
    cost(:,:,Ne)=cost1(:,:,Ne);
else
    cost(:,:,Ne)=cost2(:,:,Ne);
end

