%% Exercise "Exercise Image Stitching"
%
% by Jonas Raesch & Martin H. Trauth, 24 March 2017
% http://mres.uni-potsdam.de

%%
% First clear the workspace, the command window and close all figures
clear, clc, close all

%% Capture images using the EV3 brick
% This is the script for image acquisition. Look up the name of our camera using
% webcamlist and create a cam object.
mylego = legoev3('USB');
cam = webcam('C922 Pro Stream Webcam');
mymotor_A = motor(mylego,'A');

%%
% Because you need a large amout of overlap, we only rotate the camera by a few
% degrees. 10% speed for 0.4 seconds results in a rotation of about 4 degree. With
% speeds of less than 10 the rotation platform sometimes gets stuck. To make sure
% that the camaera comes to a stop and has time to autofocus, we included some
% pauses.
mymotor_A.Speed = 10; 
i = 1;

dur = 0.4;          % Duration
numImages = 4;      % Number of images

% Roatate the Camera, take pictures and save them
imwrite(snapshot(cam),['Image_1.png']);
while i < numImages
    tic
    while toc <= dur
        start(mymotor_A);
    end
    i = i+1;
    stop(mymotor_A);
    pause(2);
    imwrite(snapshot(cam),['Image_',num2str(i),'.png']);

end
beep(mylego);

%% Image Stitching
% You can start here without running the LEGO experiment. Please adjust the number of
% images numImages to the number of pictures you took. Then load the images from the
% hard drive.
clear, clc, close all
numImages = 4; 
for i = 1:numImages
   img(:,:,:,i) = imread(['Image_' num2str(i) '.png']);
end

%% Color adjustment
% Because the white balance, contrast and brightness differs from image to image, we
% will make some color adjustments. Calculate the mean of each colorchannel for all
% images.
for i = 1:numImages
    greyImage = rgb2gray(img(:,:,:,i));

    R = img(:,:,1,i);
    G = img(:,:,2,i);
    B = img(:,:,3,i);
    
    mR = mean2(R);
    mG = mean2(G);
    mB = mean2(B);
    mG = mean2(greyImage);

    R = uint8(double(R) * mG / mR);
    G = uint8(double(G) * mG / mG);
    B = uint8(double(B) * mG / mB);

    img(:,:,:,i) = cat(3, R, G, B);
end

%% Find matching points
% To create the panorama, you have to find matching points between the images. This
% can be done with the detectSURFFeatures command which needs a greyscale image as
% an input. 
I = img(:,:,:,1);
 
greyImage = rgb2gray(I);
points = detectSURFFeatures(greyImage);
[features, points] = extractFeatures(greyImage, points);

%%
% This has to be done for all images so we iterate through the entire image array. 
numImages = size(img);
numImages = numImages(4);
tforms(numImages) = projective2d(eye(3));

for n = 2:numImages
    
    % Save the points and features from the previous image 
    previousPoints = points;
    previousFeatures = features;
    
    % Set the current image
    I = img(:,:,:,n);
    
    greyImage = rgb2gray(I);    
    points = detectSURFFeatures(greyImage);    
    [features, points] = extractFeatures(greyImage, points);
  
    % Find matching points between the current and the previous image
    indexPairs = matchFeatures(features, previousFeatures, 'Unique', true);
       
    matchedPoints = points(indexPairs(:,1), :);
    matchedPointsPrev = previousPoints(indexPairs(:,2), :);        
    
    % Estimate the transformation between the current and the previous image
    tforms(n) = estimateGeometricTransform(matchedPoints, matchedPointsPrev,...
        'projective', 'Confidence', 99.9, 'MaxNumTrials', 2500);
  
    tforms(n).T = tforms(n-1).T * tforms(n).T; 
end

%% Creating the Panorama
% We have to create an empty panorama with the extend of all the transformed images.
% These values can be calculated with the transformation matrix. Get the size of the
% image (all the images have the same size)
sizeImage = size(I);

for i = 1:numel(tforms)           
    [xlim(i,:), ylim(i,:)] = outputLimits(tforms(i), ...
        [1 sizeImage(2)], [1 sizeImage(1)]);
end

%% 
% Find the image in the center
avgXLim = mean(xlim, 2);
[~, idx] = sort(avgXLim);
centerIdx = floor((numel(tforms)+1)/2);
centerImageIdx = idx(centerIdx);

%  Apply the inverse transformation to all images
Tinv = invert(tforms(centerImageIdx));

for i = 1:numel(tforms)    
    tforms(i).T = Tinv.T * tforms(i).T;
end

% Get the maximum extend
for i = 1:numel(tforms)           
    [xlim(i,:), ylim(i,:)] = outputLimits(tforms(i), ...
        [1 sizeImage(2)], [1 sizeImage(1)]);
end

% Get the minimum and maximum extend
xMin = min([1; xlim(:)]);
xMax = max([sizeImage(2); xlim(:)]);

yMin = min([1; ylim(:)]);
yMax = max([sizeImage(1); ylim(:)]);

% Set the extend of the panorma
width  = round(xMax - xMin);
height = round(yMax - yMin);

%% Transforming and blending the images
% We use vision.AlphaBlender to overlay the images 
blender = vision.AlphaBlender('Operation', 'Binary mask', ...
    'MaskSource', 'Input port');  

% Create a spatial object with the extend of the panorama
xLimits = [xMin xMax];
yLimits = [yMin yMax];
panoramaView = imref2d([height width], xLimits, yLimits);

% Create an empty panorama 
panorama = zeros([height width 3], 'like', I);

% Stitiching the images 
for i = 1:numImages
    
    % Read the images
    I = img(:,:,:,i);
   
    % Transform the images into the panroma using the previously calculated
    % transformation matrix, and the created view
    warpedImage = imwarp(I, tforms(i), 'OutputView', panoramaView);
                  
    % Generate a mask
    mask = imwarp(true(size(I,1),size(I,2)), tforms(i), ...
        'OutputView', panoramaView);
    
    % Add the pictures to the panorama
    panorama = step(blender, panorama, warpedImage, mask);
end

%%
% Display the panorama
imshow(panorama);

% Save the panorama and the variables in the workspace
imwrite(panorama,'Panorama.png');
save Panorama.mat
