%function RefineShape()
%
%   Estimate thee Shape Unit Parameters 
%
% Copyright 2010, Jixu Chen - Rensselaer Polytechnic Institute 
% All Rights Reserved Permission to use, copy, modify, and distribute this
% software and its documentation for any non-commercial purpose is hereby
% granted without fee, provided that the above copyright notice appear in
% all copies and that both that copyright notice and this permission
% notice appear in supporting documentation, and that the name of the
% author not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
% THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
% INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR ANY
% PARTICULAR PURPOSE. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
% SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER
% RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF
% CONTRACT, NEGLIGENCE OR OTHER TORTUOUS ACTION, ARISING OUT OF OR IN
% CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
%
% Written By:   Jixu Chen (chenj4@rpi.edu)
% Last Updated: 09/01/2010

function RefineShape(vecfile, data, startframe, out_shapefile, faceimg)
    % Frist colume : the tracked points, 
    % Second colume: corresponding CANDIDE-3 points
    Map=[2,49;
        3,50;
        4,53;
        5,56;
        6,54;
        7,55;
        9,76;
        10,64;
        11,66;
        12,86;
        13,16;
        14,17;
        15,20;
        16,23;
        17,21;
        18,22;
        20,75;
        21,31;
        22,33;
        23,85;
        24,7;
        25,8;
        26,48;
        27,15;]
     
%    ============ Load Tracked Feature Points in start frame=============
    trackpt = reshape(data(startframe*2-1,2:57),2,[]); 
    if(nargin ==5)
        figure(1);
        imshow(faceimg);hold on;
        plot(trackpt(1,:),trackpt(2,:),'r*');
    end
    trackpt(2,:) = 256 - trackpt(2,:); %for image coordinate (origin in upperleft)

%    data = load('Data\featurebond.txt');
%    trackpt = reshape(data,2,[]);


  % ========== Load CANDIDE-3 Vecs and Edges
  Vec = load(vecfile);
  %  %Vec(114,:) = (Vec(77,:) +Vec(78,:))/2;
  %  Edge = load('Data\edges');
    
    % ========= Load Shape Unit Vectors 
    shape = LoadShape();
    
%     %=================== Display CANDIDE-3 Vecs and Edges =============
% %     for i=1:length(Vec)
% %         plot(Vec(i,1),Vec(i,2),'.r'); hold on;
% %     %    text(Vec(i,1)+0.01,Vec(i,2)+0.01,num2str(i-1),'FontSize',10);
% %     end
%     Edge(:,4)=Edge(:,1); %adding a colume to form a loop;
%     for i=1:length(Edge)
%         plot(Vec(Edge(i,:)+1,1),Vec(Edge(i,:)+1,2),'-'); hold on;
%     end
%     for i=1:length(Map)
%         plot(Vec(Map(:,2)+1,1),Vec(Map(:,2)+1,2),'or','LineWidth',2); hold on;
%     end
    
    %===================  Least Square Fitting =============
    A =[];b=[];
    for i=1:length(Map)
        Matrix =[];
        Matrix(:,1)= Vec(Map(i,2)+1,1:2)';
        for j=1:11
            [tf, index] = ismember(Map(i,2),shape{j}(:,1));
            if (index>0)
                Matrix = [Matrix, shape{j}(index,2:3)']; %adding this Unit Vector
            else
                Matrix = [Matrix,zeros(2,1)];
            end
        end
        Matrix =[Matrix,eye(2)];
        
        A=[A;Matrix];
        b=[b;trackpt(:,Map(i,1)+1)];
    end
    
    x=A\b;
 
    % Refine Points by adjusting Candide shape parameters 
    refinedpt = reshape(A*x,2,[]);

    figure(2)
    plot(trackpt(1,:),trackpt(2,:),'*r'); 
    hold on;
    plot(Vec(Map(:,2)+1,1)*100,Vec(Map(:,2)+1,2)*100,'ok'); 
    plot(refinedpt(1,:),refinedpt(2,:),'ob');
    
    s = x(1);   %scale factor
    par = x(2:12)'/s; %shape parameters 
    tx = x(13); ty = x(14); %translation
    dlmwrite(out_shapefile, [s,par,tx,ty], 'precision', '%.6f', 'delimiter', '\t','newline', 'pc')
    end
        
    

