% 2014-07-31 Xiang Xiang xxiang@cs.jhu.edu

run('/home/eglxiang/Development/feaMatch/vlfeat-0.9.16/toolbox/vl_setup.m');
addpath('Algorithm', 'Visualization', 'Feature Extraction');
addpath('/home/eglxiang/Development/sinus_project/trunk/optotrakRegistration/matlab/ext/TOOLBOX_calib/');


%--- can output both HMA and SIFT feature coordinates
% Input Images
pubpath = '/home/eglxiang/Datasets/New_Sinus_R01/Data_Collection/Case_12-19-12-processed/Sample_Video_Start/resized/0';
StIdx = 3;
EndIdx = 65;
% output para
SIFTFolderName = 'Sift_May08_world';
%mkdir('/rsl', SIFTFolderName);
HMAFolderName = 'HMA_May08_world';
%mkdir('/rsl/', HMAFolderName);
% SIFT para
matching_threshold = 0.75;
% HMA para
Params = Set_HMA_parameters('SIFT');
ratio_threshold = 0.75;
model = 'A';
%--- Camera para
% Focal length:
ff = [ 836.406; 831.630 ];
 
% Principal point:
cc = [ 592.791; 534.734 ];
 
% Skew (please note Skew = Gamma/ScaleX):
alpha = 0.00413844;
 
% Distortion (radial, decentering, and thin-prism, if any)
kc = [ -0.357819 0.126829 0.00000 ];
dc = [ 0.00000 0.00000 0.00000 ];
tc = [ 0.00000 0.00000 0.00000 ];

% vector to store num of matched features for each pair
matchFeaNum = zeros(1,(EndIdx-StIdx+1));
siftMatchNum = zeros(1,(EndIdx-StIdx+1));

fileE = fopen(strcat('rsl/', HMAFolderName, '/E-matrix.txt'),'w');
fileT = fopen(strcat('rsl/', HMAFolderName, '/Tx.txt'),'w');
fileR1 = fopen(strcat('rsl/', HMAFolderName, '/R1-matrix.txt'),'w');
fileR2 = fopen(strcat('rsl/', HMAFolderName, '/R2-matrix.txt'),'w');
% write validMatchFeaNum to txt
fileFeaNum = fopen( strcat('rsl/', HMAFolderName, '/ValidMatchFeaNum.txt'), 'w' );
%fileSiftNum = fopen( strcat('rsl/', HMAFolderName, '/SiftNum.txt'), 'w' );


% read img
val_count = 0; % count of frames w/ feature num > 4
for i=StIdx:EndIdx
    try
        BName = strcat(pubpath, sprintf('%02d', i), '.bmp');
        AName = strcat(pubpath, sprintf('%02d', (i+1)), '.bmp');

        Before = imread(BName);
        After = imread(AName);

        % feature extraction
        [Polygons.Training, Features, Initial_Matches, Images] = compute_Initial_Matches(Before, After, matching_threshold, 'SIFT', []);
        saveas(gcf, strcat('rsl\', SIFTFolderName, '\SIFT_', sprintf('%03d', i)), 'jpg');
        close all;
        
        % HMA matching
        Solution = HMA_Algorithm(Features, Params, Images, Initial_Matches, ratio_threshold, model);
        
        % Visualization
        Display_Params = Set_Display_Params();
        Plot_Multi_Model(Solution, Images, Display_Params, []);
        saveas(gcf, strcat('rsl\', HMAFolderName, '\HMA_', sprintf('%03d', i)), 'jpg');
        close all;
        
        % estimate essential matrix
        % all matched fea points
        % the first affine component 
        HMA_Matches_Before = Solution.Inliers(1,1).Training(1:2, :);
        HMA_Matches_After  = Solution.Inliers(1,1).Query(1:2, :);
        % affine component from the 2nd 
        if length(Solution.Mapping) > 1
            % length(Solution.Mapping) is the num of affine components
            for j=2:length(Solution.Mapping)
                HMA_Matches_Before = [HMA_Matches_Before(1:2, :), Solution.Inliers(1,j).Training(1:2, :)];
                HMA_Matches_After  = [HMA_Matches_After(1:2, :),  Solution.Inliers(1,j).Query(1:2, :)];
            end
        end
        matchFeaNum(i-StIdx+1) = length(HMA_Matches_Before); 
        
        % convert image coordinate to World coordinate
        % set skew to zero since skew is rather small
%         HMA_Matches_Before_W = HMA_Matches_Before - repmat([cx;cy], [1 matchFeaNum(i-StIdx+1)]);
%         HMA_Matches_Before_W(1,:) = HMA_Matches_Before_W(1,:) / fx;
%         HMA_Matches_Before_W(2,:) = HMA_Matches_Before_W(2,:) / fy;                
%         HMA_Matches_After_W  = HMA_Matches_After - repmat([cx;cy], [1 matchFeaNum(i-StIdx+1)]);
%         HMA_Matches_After_W(1,:) = HMA_Matches_After_W(1,:) / fx;
%         HMA_Matches_After_W(2,:) = HMA_Matches_After_W(2,:) / fy;
        
        % rectification considering distortion
        %    [pixelCoords_out] = rectifyPixelCoordinates(pixelCoords_in,R,f,c,k,alpha,KK_new);
        R_rec = eye(3);
        kk = [ kc(1), kc(2), dc(1), dc(2), kc(3) ];
        KK_new = [ ff(1), alpha, cc(1); 0, ff(2), cc(2); 0, 0, 1 ];
        
        HMA_Matches_Before_W = rectifyPixelCoordinates(HMA_Matches_Before,R_rec,ones(2,1),zeros(2,1),kk,alpha,KK_new);
        HMA_Matches_After_W  = rectifyPixelCoordinates(HMA_Matches_After, R_rec,ones(2,1),zeros(2,1),kk,alpha,KK_new);
        
        
        % print HMA features in world coordinates to text file
        %mkdir(strcat('rsl/', HMAFolderName, '/BefFeaCoordinate'));
        fileBefFea = fopen(strcat('rsl/', HMAFolderName, '/BefFeaCoordinate/Bef_', sprintf('%04d', i) ,'.txt'), 'w');
        %mkdir(strcat('rsl/', HMAFolderName, '/AfFeaCoordinate'));
        fileAfFea  = fopen(strcat('rsl/', HMAFolderName, '/AfFeaCoordinate/Af_', sprintf('%04d', i) ,'.txt'), 'w');
        fprintf(fileBefFea, '%f\n', HMA_Matches_Before_W); % print in 'x y x y .. ' format
        fprintf(fileAfFea,  '%f\n', HMA_Matches_After_W);

                    
        % print num of initial SIFT matches
        SIFT_Matches_Before = Initial_Matches.Training(1:2, :);
        SIFT_Matches_After  = Initial_Matches.Query(1:2, :);
        if length(SIFT_Matches_Before) ~= length(SIFT_Matches_After)
            printf('Inequal Before and After SIFT features in frame %d\n', i);
            break;
        else
            numSift = size(SIFT_Matches_Before, 2);
        end
        siftMatchNum(i) = numSift;
        
        % print SIFT features in world coordinates to text file as well
        %mkdir(strcat('rsl/', HMAFolderName, '/BefFeaCoordinate'));
        % convert to world coordinate
        % set skew to zero since skew is rather small
%         SIFT_Matches_Before_W = SIFT_Matches_Before - repmat([cx;cy], [1 numSift]);
%         SIFT_Matches_Before_W(1,:) = SIFT_Matches_Before_W(1,:) / fx;
%         SIFT_Matches_Before_W(2,:) = SIFT_Matches_Before_W(2,:) / fy;                
%         SIFT_Matches_After_W = SIFT_Matches_After - repmat([cx;cy], [1 numSift]);
%         SIFT_Matches_After_W(1,:) = SIFT_Matches_After_W(1,:) / fx;
%         SIFT_Matches_After_W(2,:) = SIFT_Matches_After_W(2,:) / fy;
        SIFT_Matches_Before_W = rectifyPixelCoordinates(SIFT_Matches_Before,R_rec,ones(2,1),zeros(2,1),kk,alpha,KK_new);
        SIFT_Matches_After_W  = rectifyPixelCoordinates(SIFT_Matches_After, R_rec,ones(2,1),zeros(2,1),kk,alpha,KK_new);
        
        fileBefSift = fopen(strcat('rsl/', SIFTFolderName, '/BefSiftCoordinate/Bef_', sprintf('%04d', i) ,'.txt'), 'w');
        %mkdir(strcat('rsl/', HMAFolderName, '/AfFeaCoordinate'));
        fileAfSift = fopen(strcat('rsl/', SIFTFolderName, '/AfSiftCoordinate/Af_', sprintf('%04d', i) ,'.txt'), 'w');
        fprintf(fileBefSift, '%f\n', SIFT_Matches_Before_W); % print in 'x y x y .. ' format
        fprintf(fileAfSift,  '%f\n', SIFT_Matches_After_W);
        
        if matchFeaNum(i-StIdx+1) > 4
            %-- valid frame index
            %val_count = val_count + 1;
            %validMatchFeaNum(val_count) = matchFeaNum(i-StIdx+1);
            fprintf( fileFeaNum, '%d\n', matchFeaNum(i-StIdx+1) );
            
            %fprintf( fileSiftNum, '%d\n', siftMatchNum(i-StIdx+1) );
            
            % five point algorithm 
            Evec = calibrated_fivepoint( HMA_Matches_Before_W', HMA_Matches_After_W');

            for k=1:size(Evec,2)
               E = reshape(Evec(:,k),3,3);
               %det( E)
               %2 *E*transpose(E)*E -trace( E*transpose(E))*E
            end
            fprintf(fileE, 'Frame %d - %d\n', i, (i+1));
            fprintf(fileE, '%f\n', E);
            fprintf(fileE,  '\n\n');

            % (by Xiang) decompose E to R,t
            [Tx,R1,R2] = torr_EtoRt(E);

            fprintf(fileT, 'Frame %d - %d\n', i, (i+1));
            fprintf(fileT, '%f\n', Tx);
            fprintf(fileT,  '\n\n');

            fprintf(fileR1, 'Frame %d - %d\n', i, (i+1));
            fprintf(fileR1, '%f\n', R1);
            fprintf(fileR1, '\n\n');
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                         

            fprintf(fileR2, 'Frame %d - %d\n', i, (i+1));
            fprintf(fileR2, '%f\n', R2);
            fprintf(fileR2, '\n\n');
        end
        
        clear BName;
        clear AName;
        clear Before;
        clear After;
        clear HMA_Matches_Before;
        clear HMA_Matches_After;
        clear HMA_Matches_Before_W;
        clear HMA_Matches_After_W;
        clear SIFT_Matches_Before;
        clear SIFT_Matches_After;
        clear SIFT_Matches_Before_W;
        clear SIFT_Matches_After_W;
        close all; % close all figures
    catch err
        fprintf(strcat('error when i = ', int2str(i), '\n'));
    end
end


fclose('all');

plotx = StIdx:1:EndIdx;
figure, plot(plotx, matchFeaNum, '--rs', 'LineWidth',2, 'MarkerEdgeColor','k', ...
    'MarkerFaceColor','g', 'MarkerSize', 10);
saveas(gcf, strcat('rsl\', HMAFolderName, '\PlotMatchFea.jpg'));
