%  outlierRemoveT:  outlier removal 
%                   using delaunay triangulation for localisation
%                     triangle flip and displacement magnitude & direction
%                     to determine outlier
%
%
%  Usage:      [xy1_or,xy2_or] = outlierRemoveT(xy1,xy2,figNo,im1,im2,debug)
%
%  Arguments:   xy1     - first set of points
%               xy2     - second set of points
%                           row 1: x coordinates, 
%                           row 2: y coordinates
%  [Optional]
%               figNo   - figNo <= 0 : no figure display
%                         figNo >  0 : display figure while code runs
%                         figNo = -1 : show figure at last (faster)
%               im1,im2 - two images where the point correspondance is derived
%               debug   - 1 : show lots of details for debugging.
%
%  Returns:     xy1_or - xy1 with outlier removed
%               xy2_or - xy2 with outlier removed
%
% Author: 
% Tzu Yen Wong
% wongt AT csse DOT uwa DOT edu DOT au
% Department of Computer Science & Software Engineering
% The University of Western Australia
%
% May 2004
% June 2004 change email address to fight SPAM!!!

% Example using result from 'testfund' of RANSAC from PK
% [xy1,xy2] = outlierRemoveT(...
%               [m1(2,inliers);m1(1,inliers)],...
%               [m2(2,inliers);m2(1,inliers)],1,im1,im2);

%1234567890123456789012345678901234567890123456789012345678901234567890123456789

function [xy1_or,xy2_or] = outlierRemoveT(xy1,xy2,figNo,im1,im2,debug)

% xy1 & xy2 : row 1 is x, row 2 is y

% MMMMMMMMMM initialisation & Error check MMMMMMMMMM
eval('figNo;','figNo=0;');        eval('debug;','debug=0;');
totalUVectorSum = 0;                    totalScalarSum = 0;
outlierTriP =[];
count =1;           count2 =1;          count3 =1;
dummyOutlierZeros = ones(size(xy1(1,:)));

if ~all(size(xy1)==size(xy2)),
    error('size of two sets of points has to be equal!')
end
% MMMMM End initialisation & Error check MMMMM

% Triangulation of only the first set of points 
% the second set should be able to use the same triangulation
% otherwise there's outlier
tri = delaunay(xy1(1,:),xy1(2,:));

[noOfTri,three] = size(tri);

distortion = zeros(noOfTri,4);          
distortion(:,1)=[1:noOfTri]';

% DDDDDDDDDDDDDDDDDDDD Display DDDDDDDDDDDDDDDDDDDD
% initialising figure before going into loop
if figNo >0,
    maxY = max(max(xy1(2,:),xy2(2,:)));
    maxX = max(max(xy1(1,:),xy2(1,:)));
    eval('im1;im2;','im1=zeros(maxY,maxX);im2=im1;')
    show(double(im1)+double(im2),figNo), hold on
    show(double(im1)+double(im2),figNo+1), hold on
    % DoubleBuffer prevent image flickering when it's updated.
    set(figNo,'name','Original Triangulation','DoubleBuffer','on')
	set(figNo+1,'name','Outlier Triangulation','DoubleBuffer','on')
end
% DDDDDDDDDDDDDDDDDD End Display DDDDDDDDDDDDDDDDDD

% MMMMMMMMMM Looping through all Triangles MMMMMMMMMM
for tri_i = 1:noOfTri,

    % Grabbing triangle points coordinate for first set (set 1)
    p1_1 = [xy1(1,tri(tri_i,1)) xy1(2,tri(tri_i,1))];
    p1_2 = [xy1(1,tri(tri_i,2)) xy1(2,tri(tri_i,2))];
    p1_3 = [xy1(1,tri(tri_i,3)) xy1(2,tri(tri_i,3))];
    % Grabbing triangle points coordinate for corresponding set (set 2)
    p2_1 = [xy2(1,tri(tri_i,1)) xy2(2,tri(tri_i,1))];
    p2_2 = [xy2(1,tri(tri_i,2)) xy2(2,tri(tri_i,2))];
    p2_3 = [xy2(1,tri(tri_i,3)) xy2(2,tri(tri_i,3))];
    
    % vector of triangle side set 1 
    t1v12 = p1_2 - p1_1;   % point 1->2
    t1v13 = p1_3 - p1_1;
    t1v32 = p1_2 - p1_3;
    % vector of triangle side set 2 
    t2v12 = p2_2 - p2_1;   % point 1->2
    t2v13 = p2_3 - p2_1;
    t2v32 = p2_2 - p2_3;
    
    % dot product of unit vector between two sets' corresponding sides
    % dotAB measure the level of parellel between point A & B
    dot12 = dot(t1v12/norm(t1v12),t2v12/norm(t2v12));
    dot13 = dot(t1v13/norm(t1v13),t2v13/norm(t2v13));
    dot32 = dot(t1v32/norm(t1v32),t2v32/norm(t2v32));
    
    dotArray = [ dot32 dot13 dot12 ];
    distortion(tri_i,2:4) = dotArray;

    % vector of point shift from set 1 -> set 2
    v1 = p2_1 - p1_1;   
    v2 = p2_2 - p1_2;
    v3 = p2_3 - p1_3;
    % magnitude of the vector
    magv1 = norm(v1);
    magv2 = norm(v2);
    magv3 = norm(v3);
    % sum of all three for global comparison later
    scalerSum  = magv1+magv2+magv3;
    uVectorSum  = v1/magv1 + v2/magv2 + v3/magv3;
    
    % DDDDDDDDDDDDDDDDDDDD Display DDDDDDDDDDDDDDDDDDDD
	if figNo>0,
        P1 = [p1_1;p1_2;p1_3;p1_1];
        P2 = [p2_1;p2_2;p2_3;p2_1];
        figure(figNo), hold on
    
            plot(P1(:,1),P1(:,2),'+r-'), 
            plot(P2(:,1),P2(:,2),'+g-'), 
            plot([p1_1(1) p2_1(1)],[p1_1(2) p2_1(2)],'b:')
            plot([p1_2(1) p2_2(1)],[p1_2(2) p2_2(2)],'b:')
            plot([p1_3(1) p2_3(1)],[p1_3(2) p2_3(2)],'b:')
            title(sprintf('Triangle "%d" of %d, point %d %d %d',...
                tri_i,noOfTri,...
                tri(tri_i,1),tri(tri_i,2),tri(tri_i,3)))
        drawnow
	end % figNo>0
    % DDDDDDDDDDDDDDDDDD End Display DDDDDDDDDDDDDDDDDD
    
    % AAAAAAAAAAAAAAAAAAAA Outlier Triangle ? AAAAAAAAAAAAAAAAAAAA
    % angle between corresponding sides > 90 deg ==> Outlier
    if any(dotArray < cos(pi/2)), % Outlier triangle found!!
        
        % For two corresponding triangles with points abc and ABC
        % already classified as outlier triangle,
        % A<-->a correspondance will be classified the outlier point if
        % bB & cC are more parellel than aA & bB and aA & cC

		% outlier triangle index to 'tri'
        outlierTri(count) = tri_i;
        
        % outlier triangle point index to xy1 & xy2, 
        %   first column is the outlier point of triangle tri_i
        outlierTriP(count,1)   = tri(tri_i,find(dotArray==max(dotArray)));
        outlierTriP(count,2:3) = tri(tri_i,find(dotArray~=max(dotArray)));
        
        % DDDDDDDDDDDDDDDDDDDD Display DDDDDDDDDDDDDDDDDDDD
        if figNo>0,
            plot([p1_1(1) p2_1(1)],[p1_1(2) p2_1(2)],'ob-')
            plot([p1_2(1) p2_2(1)],[p1_2(2) p2_2(2)],'ob-')
            plot([p1_3(1) p2_3(1)],[p1_3(2) p2_3(2)],'ob-')
            plot(P1(:,1),P1(:,2),'y:'), 
            plot(P2(:,1),P2(:,2),'y:'), %hold off
            axisValue = axis;
            figure(figNo+1), hold on
                axis(axisValue);
                axis image
                plot([p1_1(1) p2_1(1)],[p1_1(2) p2_1(2)],'b:')
                plot([p1_2(1) p2_2(1)],[p1_2(2) p2_2(2)],'b:')
                plot([p1_3(1) p2_3(1)],[p1_3(2) p2_3(2)],'b:')
                plot(P1(:,1),P1(:,2),'oy-'), 
                plot(P2(:,1),P2(:,2),'xc-'), %hold off
        end % figNo>0
        
        if count ==1,
            disp('potential outlier points :')
            disp('main suspect on first column')
            disp(' v')
        end
        
        disp(sprintf(...
            '%3d %3d %3d : [%3d,%3d]->[%3d,%3d],[%3d,%3d]->[%3d,%3d],[%3d,%3d]->[%3d,%3d]',...
            outlierTriP(count,1),outlierTriP(count,2),outlierTriP(count,3),...
            p1_1(1),p1_1(2),p2_1(1),p2_1(2),...
            p1_2(1),p1_2(2),p2_2(1),p2_2(2),...
            p1_3(1),p1_3(2),p2_3(1),p2_3(2) ))
        % DDDDDDDDDDDDDDDDDD End Display DDDDDDDDDDDDDDDDDD
        
        count = count+1;
%        pause 
	else % Not Outlier
        totalUVectorSum = totalUVectorSum + uVectorSum;
        totalScalarSum = totalScalarSum + scalerSum;
    end % any(dotArray <0)
    % AAAAAAAAAAAAAAAAAA End Outlier Triangle ? AAAAAAAAAAAAAAAAAA
    
end % for tri_i = 1:noOfTri
% MMMMMMMM End Looping through all Triangles MMMMMMMM

if debug~=0,    distortion,  end
disp(' ')

% AAAAAAAAAAAAAAAAAAAA Outlier Triangle Points Process AAAAAAAAAAAAAAAAAAAA
if length(outlierTriP)~=0,
	maxOutlierTriP = max(outlierTriP(:));

    % tally of possible outlier points indices and their frequencies
    % 0.5 and max+0.5 added to center bin on integer value
	[N,X] = hist([0.5;outlierTriP(:);maxOutlierTriP+0.5],maxOutlierTriP);
    % fixed up unwanted count of 0.5 and max+0.5 
	N(1)=N(1)-1; N(end)=N(end)-1; 
	
    % clean up indices that didn't occur, using subfunction at the end
    % histogram(2,:) is potential outlier points' indices to xy1 & xy2
    % histogram(1,:) is frequency of occurrance
	histogram = stripZeros(N,[N;X]); 
    % sum(N)/3 because each triangle has 3 points
    avgUVector = totalUVectorSum/(length(distortion)-sum(N)/3);
    avgMag     = totalScalarSum/(length(distortion)-sum(N)/3);

	if debug~=0,    histogram,  end
    
    
    % MMMMMMMMMM Loop Through All Potential Outlier Indices MMMMMMMMMM
    % First Outlier Check -- displacement - magnitude and direction
	for ii = 1:length(histogram(2,:)),  

        p1 = [xy1(1,histogram(2,ii)) xy1(2,histogram(2,ii))];
        p2 = [xy2(1,histogram(2,ii)) xy2(2,histogram(2,ii))];
        
        v = p2 - p1;   % vector of point shift
        magv = norm(v);
        
        % AAAAAAAAAAAAAAA Displacement Not Right ? AAAAAAAAAAAAAAA
        % IF double scalar size OR angle difference > pi/4  THEN Outlier
        if (abs(magv-avgMag)/avgMag)>1 || dot(v/magv,avgUVector)<cos(pi/4), 
            
            outlier(count2) = histogram(2,ii);
			count2 = count2+1;
            dummyOutlierZeros(histogram(2,ii))=0;

            % DDDDDDDDDDDDDDDDDDDD Display DDDDDDDDDDDDDDDDDDDD
            if figNo>0,
                figure(figNo+1), hold on
                plot([p1(1) p2(1)],[p1(2) p2(2)],'xr:')
            end
            disp(sprintf(...
                'Outlier found on %3d : [%3d,%3d]->[%3d,%3d] %s',...
                histogram(2,ii),p1(1),p1(2),p2(1),p2(2),...
                '-- displacement check'))
            % DDDDDDDDDDDDDDDDDD End Display DDDDDDDDDDDDDDDDDD
            
        else % magnitude and direction OK
            passFirstTest(count3) = histogram(2,ii);
            count3 = count3+1;
        end % (abs(magv-avgMag)/avgMag)>1 || dot(v/magv,avgUVector)<cos(pi/4)
        % AAAAAAAAAAAAA End Displacement Not Right ? AAAAAAAAAAAAA
        
    end % ii = 1:length(histogram(2,:)) 
    % MMMMMMMM End Loop Through All Potential Outlier Indices MMMMMMMM

    
    % MMMMMMMMMM Loop Through Remaining Potential Outlier Indices MMMMMMMMMM
    % Second Outlier Check -- Triangle Shape Change
    %   (dot product of corresponding sides)
	for aa = 1:length(passFirstTest),   
        p1 = [xy1(1,passFirstTest(aa)) xy1(2,passFirstTest(aa))];
        p2 = [xy2(1,passFirstTest(aa)) xy2(2,passFirstTest(aa))];
        
        % outlierTriP(:,1) is the outlier points of outlier triangles
        rowIndexOfSuspect = find(outlierTriP(:,1)==passFirstTest(aa));

        if ~isempty(rowIndexOfSuspect),
            
            otherPointsInTri = outlierTriP(rowIndexOfSuspect,2:3);
            otherPointsInTriIsOutlier = zeros(size(otherPointsInTri));
            for jj=1:length(outlier),
                otherPointsInTriIsOutlier = otherPointsInTriIsOutlier...
                                    | otherPointsInTri==outlier(jj);
            end
            
            % a valid point in outlier triangle can be classified as outlier
            % if the other two points are outliers

            % check each row first (dim2) then all column (dim1)
            if ~all(all(otherPointsInTriIsOutlier,2),1), % outlier point found
				outlier(count2) = passFirstTest(aa);
				count2 = count2+1;
                dummyOutlierZeros(passFirstTest(aa))=0;
                
                % DDDDDDDDDDDDDDDDDDDD Display DDDDDDDDDDDDDDDDDDDD
                if figNo>0,
                    figure(figNo+1), hold on
                    plot([p1(1) p2(1)],[p1(2) p2(2)],'xr:')
                end
                disp(sprintf(...
                    'Outlier found on %3d : [%3d,%3d]->[%3d,%3d] %s',...
                passFirstTest(aa),p1(1),p1(2),p2(1),p2(2),...
                '-- triangle check'))
                % DDDDDDDDDDDDDDDDDD End Display DDDDDDDDDDDDDDDDDD
                
            end % ~all(all(otherPointsInTriIsOutlier,2),1)
        end % any(outlierTriP(:,1)==passFirstTest(aa))
    end % aa = 1:length(passFirstTest)
    % MMMMMMMM End Loop Through Remaining Potential Outlier Indices MMMMMMMM
    
    % composing inliner by taking off outlier
    xy1_or = stripZeros(dummyOutlierZeros,xy1);
	xy2_or = stripZeros(dummyOutlierZeros,xy2);
	
else % No Outlier found
    disp('No Outlier found');
    xy1_or = xy1;
    xy2_or = xy2;
    avgUVector = totalUVectorSum/(length(distortion));  % 
    avgMag     = totalScalarSum/(length(distortion));
end % length(outlierTriP)~=0,
% AAAAAAAAAAAAAAAAAA End Outlier Triangle Points Process AAAAAAAAAAAAAAAAAA


% DDDDDDDDDDDDDDDDDDDD Display DDDDDDDDDDDDDDDDDDDD
% special display for quick result
% figure 1 show the original triangulation with possible outlier
% figure 2 show the outlier removed triangulation 
% when any key is pressed, 
%   a registered version of the correspondance are shown
% when any key is again pressed,
%   the figures are close.
if figNo == -1,
    maxY = max(max(xy1(2,:),xy2(2,:)));
    maxX = max(max(xy1(1,:),xy2(1,:)));
    eval('im1;im2;','im1=zeros(maxY,maxX);im2=im1;')
    show(double(im1)+double(im2),1), hold on
    show(double(im1)+double(im2),2), hold on
    
	figure(1), set(1,'name','Original Triangulation')
	plottriangle(xy1(1,:),xy1(2,:),tri,'cc')
	plottriangle(xy2(1,:),xy2(2,:),tri,'yy')
    for iii = 1:length(xy1),
        plot([xy1(1,iii) xy2(1,iii)],...
             [xy1(2,iii) xy2(2,iii)],'b:')
    end
    
    eval('outlier;','outlier=[];')
    for jjj = 1:length(outlier)
        plot([xy1(1,outlier(jjj)) xy2(1,outlier(jjj))],...
             [xy1(2,outlier(jjj)) xy2(2,outlier(jjj))],'xr:')
    end
    
    figure(2), set(2,'name','Outlier Removed Triangulation')
    
    inline_tri = delaunay(xy1_or(1,:),xy2_or(2,:));
    
	plottriangle(xy1_or(1,:),xy1_or(2,:),inline_tri,'cc')
	plottriangle(xy2_or(1,:),xy2_or(2,:),inline_tri,'yy')
    for kkk = 1:length(xy1_or),
        plot([xy1_or(1,kkk) xy2_or(1,kkk)],...
             [xy1_or(2,kkk) xy2_or(2,kkk)],'b:')
    end
    
    pause,
    
    figure(1), clf, set(1,'name','Registered Original Triangulation')
    show(im1,1), hold on
	plottriangle(xy1(1,:),xy1(2,:),tri,'cc')
	plottriangle(xy2(1,:)-avgMag*avgUVector(1)/9 ,...
                 xy2(2,:)-avgMag*avgUVector(2)/9,tri,'yy')
    for iii = 1:length(xy1),
        plot([xy1(1,iii) xy2(1,iii)-avgMag*avgUVector(1)/9],...
             [xy1(2,iii) xy2(2,iii)-avgMag*avgUVector(2)/9],'b:')
    end
    eval('outlier;','outlier=[];')
    for jjj = 1:length(outlier)
        plot([xy1(1,outlier(jjj)) ...
                xy2(1,outlier(jjj))-avgMag*avgUVector(1)/9],...
             [xy1(2,outlier(jjj)) ...
                xy2(2,outlier(jjj))-avgMag*avgUVector(2)/9],...
             'xr:')
    end
    
    figure(2), clf, set(2,'name','Registered Outlier Removed Triangulation')
    show(im1,2), hold on
	plottriangle(xy1_or(1,:),xy1_or(2,:),inline_tri,'cc')
	plottriangle(xy2_or(1,:)-avgMag*avgUVector(1)/9,...
                 xy2_or(2,:)-avgMag*avgUVector(2)/9,inline_tri,'yy')
    for kkk = 1:length(xy1_or),
        plot([xy1_or(1,kkk) xy2_or(1,kkk)-avgMag*avgUVector(1)/9],...
             [xy1_or(2,kkk) xy2_or(2,kkk)-avgMag*avgUVector(2)/9],'b:')
    end
    
    pause
    close(1),close(2)
end % figNo == -1
% DDDDDDDDDDDDDDDDDD End Display DDDDDDDDDDDDDDDDDD

% FFFFFFFFFFFFFFFFFFFF Subfunction FFFFFFFFFFFFFFFFFFFF
function strippedArray = stripZeros(vectorWithZeros,matchingArray)
    [rZ,cZ] = size(vectorWithZeros);
    [rM,cM] = size(matchingArray);
    if rZ ==1, % one row
        if cZ ~=cM, % NOT same column number
            error(sprintf('Unmatch matrix! [%d,%d] <-> [%d,%d]',...
                rZ,cZ,rM,cM))
        end
    elseif cZ == 1, % one column
        if rZ ~=rM, % NOT same row number
            error(sprintf('Unmatch matrix! [%d,%d] <-> [%d,%d]',...
                rZ,cZ,rM,cM))
        else % transpose to be matching on columns
            vectorWithZeros = vectorWithZeros';
            matchingArray = matchingArray';
            [rM,cM] = size(matchingArray);
        end
    else % neither one row nor one column!!
        error(sprintf('Not a vector! [%d,%d]',rZ,cZ))
    end
    
    nonZeroIndex = find(vectorWithZeros~=0);
    strippedArray = zeros(rM,length(nonZeroIndex));

    for ii = 1:length(nonZeroIndex),
        strippedArray(:,ii) = matchingArray(:,nonZeroIndex(ii));
    end
return
% FFFFFFFFFFFFFFFFFFF End Subfunction FFFFFFFFFFFFFFFFFF