% Sample use of PointCloud2Image(...)
% 
% The following variables are contained in the provided data file:
%       BackgroundPointCloudRGB,ForegroundPointCloudRGB,K,crop_region,filter_size
% None of these variables needs to be modified


clc
clear all
% load variables: BackgroundPointCloudRGB,ForegroundPointCloudRGB,K,crop_region,filter_size)
load data.mat

data3DC = {BackgroundPointCloudRGB,ForegroundPointCloudRGB};
R       = eye(3);
move    = [0 0 -0.3]';
start_t = [0,0,-1.33]';
t0 = start_t;
f_orig_x    = K(1,1);
f_orig_y    = K(2,2);

for step=0:75
    tic
    K1 = 1 - 2*step/75;
    fname       = sprintf('SampleOutput%03d.jpg',step);
    display(sprintf('\nGenerating %s',fname));
    t           = start_t + step * move;
    ratio_dist = (2.67-step*0.003)/2.67;
    K(1,1) = f_orig_x * ratio_dist;K(2,2) = f_orig_y * ratio_dist;
    M           = K*[R t];
    im          = PointCloud2Image(M,data3DC,crop_region,filter_size);
    imwrite(im,fname);
    
    new_im = zeros(size(im));
    for size_x = 1:size(im,1)
        for size_y = 1:size(im,2) 
            pix_x = (size_x - size(im,1)/2)/size(im,1)*2;
            pix_y = (size_y - size(im,2)/2)/size(im,2)*2;
            radius2 = pix_x*pix_x + pix_y*pix_y;
            pix_x = (1+K1*radius2) * pix_x;
            pix_y = (1+K1*radius2) * pix_y;
            new_pix_x = pix_x*size(im,1)/2 + size(im,1)/2;
            new_pix_y = pix_y*size(im,2)/2 + size(im,2)/2;
            if new_pix_x >= 1 & new_pix_x <= size(im,1) & new_pix_y >= 1 & new_pix_y <= size(im,2)
                new_im(uint16(new_pix_x),uint16(new_pix_y),:) = im(size_x,size_y,:);     
            end
        end
    end
    SE = strel('square',3);
    BW2 = imdilate(new_im,SE);
    imwrite(BW2,['Distorted' mat2str(step) '.jpg']);
    toc    
end

