ld = load("cameraParams.mat");

numCameras = 8;
intrinsics = cell(numCameras, 1);

intrinsics(:) = {ld.cameraParams.Intrinsics};

% Measurements in meters
patternOriginHeight = 0.625;
squareSize = 29e-3;

patternPositions = ["front", "left" , "left" , "back" ,...
                    "back" , "right", "right", "front"];
extrinsicsCalibrationImages = cell(1, numCameras);
for i = 1:numCameras
    filename = "extrinsicsCalibrationImage" +  string(i) + ".jpg";
    extrinsicsCalibrationImages{i} = imread(filename);
end
helperVisualizeScene(extrinsicsCalibrationImages, patternPositions)

monoCams = cell(1, numCameras);
for i = 1:numCameras
    % Undistort the image.
    undistortedImage = undistortImage(extrinsicsCalibrationImages{i}, intrinsics{i});
    
    % Detect checkerboard points.
    [imagePoints, boardSize] = detectCheckerboardPoints(undistortedImage,...
                                                        "PartialDetections", false);
    
    % Generate world points of the checkerboard.
    worldPoints = generateCheckerboardPoints(boardSize, squareSize);
    
    % Estimate extrinsic parameters of the monocular camera.
    [pitch, yaw, roll, height] = estimateMonoCameraParameters(intrinsics{i}, ...
                                     imagePoints, worldPoints, patternOriginHeight,...
                                     "PatternPosition", patternPositions(i));
    
    % Create a monoCamera object, assuming the camera is at origin.
    monoCams{i} = monoCamera(intrinsics{i}, height, ...
                             "Pitch", pitch, ...
                             "Yaw"  , yaw, ...
                             "Roll" , roll, ...
                             "SensorLocation", [0, 0]);
end

sceneImages = cell(1, numCameras);
for i = 1:numCameras
    filename = "sceneImage" + string(i) + ".jpg";
    sceneImages{i} = imread(filename);
end
helperVisualizeScene(sceneImages)

distFromVehicle = 4.5; % in meters
outView = [-distFromVehicle, distFromVehicle, ... % [xmin, xmax,
           -distFromVehicle, distFromVehicle];    %  ymin, ymax]
outImageSize = [640, NaN];

bevImgs = cell(1, numCameras);
birdsEye = cell(1, numCameras);
for i = 1:numCameras
    undistortedImage = undistortImage(sceneImages{i}, monoCams{i}.Intrinsics);
    birdsEye{i} = birdsEyeView(monoCams{i}, outView, outImageSize);
    bevImgs{i}  = transformImage(birdsEye{i}, undistortedImage);    
end
helperVisualizeScene(bevImgs)

tiled360DegreesBirdsEyeView = zeros(640, 640, 3);
for i = 1:numCameras
    tiled360DegreesBirdsEyeView = helperBlendImages(tiled360DegreesBirdsEyeView, bevImgs{i});
end
figure 
imshow(tiled360DegreesBirdsEyeView)

% The last two images of the scene best demonstrate the advantage of
% constrained feature matching as they have many repetitive pavement
% markings.
I = bevImgs{7};
J = bevImgs{8};

% Extract features from the two images.
grayImage = rgb2gray(I);
pointsPrev = detectKAZEFeatures(grayImage);
[featuresPrev, pointsPrev] = extractFeatures(grayImage, pointsPrev);

grayImage = rgb2gray(J);
points = detectKAZEFeatures(grayImage);
[features, points] = extractFeatures(grayImage, points);

% Match features using the two methods.
indexPairs1 = matchFeaturesInRadius(featuresPrev, features, points.Location, ...
                                   pointsPrev.Location, 15, ...
                                   "MatchThreshold", 10, "MaxRatio", 0.6);

indexPairs2 = matchFeatures(featuresPrev, features, "MatchThreshold", 10, ...
                            "MaxRatio", 0.6);

% Visualize the matched features.
tiledlayout(1,2)
nexttile
showMatchedFeatures(I, J, pointsPrev(indexPairs1(:,1)), points(indexPairs1(:,2)))
title(sprintf('%d pairs matched\n with spatial constraints', size(indexPairs1, 1)))

nexttile
showMatchedFeatures(I, J, pointsPrev(indexPairs2(:,1)), points(indexPairs2(:,2)))
title(sprintf('%d pairs matched\n without spatial constraints', size(indexPairs2,1)))

% Combine the first four images to get the stitched leftSideview and the
% spatial reference object Rleft.
radius = 15;
leftImgs = bevImgs(1:4);
tforms = helperRegisterImages(leftImgs, radius);
[leftSideView, Rleft] = helperStitchImages(leftImgs, tforms);
 
% Combine the last four images to get the stitched rightSideView.
rightImgs = bevImgs(5:8);
tforms = helperRegisterImages(rightImgs, radius);
rightSideView = helperStitchImages(rightImgs, tforms);
 
% Combine the two side views to get the 360° bird's-eye-view in
% surroundView and the spatial reference object Rsurround
radius = 50;
imgs = {leftSideView, rightSideView};
tforms = helperRegisterImages(imgs, radius);
[surroundView, Rsurround] = helperStitchImages(imgs, tforms);
figure
imshow(surroundView)

proximityRange = [2, 3, 4]; % in meters
colors         = ["red", "yellow", "green"];
refBirdsEye    = birdsEye{1};
Rout           = {Rleft, Rsurround};
vehicleCenter  = [0, 0];
vehicleCenterInImage = helperVehicleToBirdsEyeView(refBirdsEye, vehicleCenter, Rout);

for i = 1:length(proximityRange)
    
    % Estimate the radius of the circular guidelines in pixels given its
    % radius in meters.
    circlePoint         = [0, proximityRange(i)];
    circlePointInImage  = helperVehicleToBirdsEyeView(refBirdsEye, circlePoint, Rout);        
    
    % Compute radius using euclidean norm.
    proximityRangeInPixels  = norm(circlePointInImage - vehicleCenterInImage, 2);
    
    surroundView = insertShape(surroundView, "Circle", [vehicleCenterInImage, proximityRangeInPixels], ...
                               "LineWidth", 1, ...
                               "Color", colors(i));
    
    labelText = string(proximityRange(i)) + " m";
    surroundView = insertText(surroundView, circlePointInImage, labelText,...
                              "TextColor", "White", ...
                              "FontSize", 14, ...
                              "BoxOpacity", 0);
end

imshow(surroundView)

vehicleCenter = [0, 0];
vehicleSize   = [5.6, 2.4]; % length-by-width in meters
[polygonPoints, vehicleLength, vehicleWidth] = helperGetVehicleBoundaryOnBEV(refBirdsEye, ...                                                                             
                                                                             vehicleCenter, ...
                                                                             vehicleSize, ...
                                                                             Rout);
showShape("polygon", polygonPoints, "Label", "Ego Vehicle")

% Read the picture of the simulation vehicle.
egoVehicle = imread("vehicle.png", "BackgroundColor", [0 0 0]);

% Bring the simulation vehicle into the vehicle coordinate system.
egoVehicle = imresize(egoVehicle, [vehicleLength, vehicleWidth]);
vehicle    = zeros(size(surroundView), "uint8");
xIdx       = polygonPoints(1,1) + (1:vehicleWidth);
yIdx       = polygonPoints(1,2) + (1:vehicleLength);
vehicle(yIdx, xIdx, :) = egoVehicle;

% Overlay the simulation vehicle on the 360° bird's-eye-view image.
sceneBirdsEyeView = helperOverlayImage(vehicle, surroundView); 

distFromVehicle = 4.25; % in meters
[x, y, h, w] = helperGetImageBoundaryOnBEV(refBirdsEye, distFromVehicle, Rout);
croppedSceneBirdsEyeView = imcrop(sceneBirdsEyeView, [x, y, h, w]);
imshow(croppedSceneBirdsEyeView)

function helperVisualizeScene(images, varargin)
    
    numImages = numel(images);    
    if nargin == 2
        titleText = varargin{1};
    else
        titleText = strings(numImages,1);
    end
    
    % Define index locations to simulate the camera positions relative to the vehicle.
    cameraPosInFigureWindow = [1, 4, 7, 8, 9, 6, 3, 2];
    egoVehiclePosInFigureWindow = 5;
    
    figure
    t = tiledlayout(3, 3, "TileSpacing", "compact", "Padding", "compact");    
    for i = 1:numImages
        nexttile(cameraPosInFigureWindow(i)) 
        imshow(images{i})
        title(titleText(i))
    end
    
    % Visualize the vehicle on the scene.
    egoVehicle = imread("vehicle.png", "BackgroundColor", [1 1 1]);
    nexttile(egoVehiclePosInFigureWindow)
    imshow(egoVehicle)
    if nargin == 2
        title("Ego Vehicle")
        title(t, "Pattern positions");
    end
end

function outputImage = helperBlendImages(I1, I2) 
    arguments
        I1 uint8
        I2 uint8
    end
    % Identify the image regions in the two images by masking out the black
    % regions.
    mask1 = sum(I1, 3) ~= 0;
    mask2 = sum(I2, 3) ~= 0;
    maskc = mask1 & mask2;
    
    % Compute alpha values that are proportional to the center seam of the two
    % images.
    alpha1 = ones(size(mask1,1:2));
    alpha2 = ones(size(mask2,1:2));
    dist1  = bwdist(edge(mask1));
    dist2  = bwdist(edge(mask2));
    alpha1(maskc) = double(dist1(maskc) > dist2(maskc));    
    alpha2(maskc) = double(dist1(maskc) <= dist2(maskc));
    
    I1 = double(I1);
    I2 = double(I2);        
    outputImage = alpha1.*I1 + alpha2.*I2;    
    outputImage = uint8(outputImage);
end

function tforms = helperRegisterImages(images, radius)
    
    params.Radius         = radius;
    params.MatchThreshold = 10;
    params.MaxRatio       = 0.6;
    params.Confidence     = 99.9;
    params.MaxDistance    = 2;
    params.MaxNumTrials   = 2000;
    
    numImages = numel(images);
    
    % Store points and features for all images.
    features = cell(1,numImages);
    points   = cell(1,numImages);
    for i = 1:numImages
        grayImage = rgb2gray(images{i});
        points{i} = detectKAZEFeatures(grayImage);
        [features{i}, points{i}] = extractFeatures(grayImage, points{i});
    end

    % Initialize all the transforms to the identity matrix.
    tforms(numImages) =  affine2d(eye(3));
    
    % Set the seed for reproducibility.
    rng(0);
    
    % Find the relative transformations between each image pair.
    for i = 2:numImages
        % Find correspondences between images{i} and images{i-1} using
        % constrained feature matching.
        indexPairs = matchFeaturesInRadius(features{i-1}, features{i}, points{i}.Location, ...
                                           points{i-1}.Location, params.Radius, ...
                                           "MatchThreshold", params.MatchThreshold, ...
                                           "MaxRatio", params.MaxRatio);
        
        %  Estimate the transformation between images{i} and images{i-1}.
        matchedPointsPrev = points{i-1}(indexPairs(:,1), :);    
        matchedPoints     = points{i}(indexPairs(:,2), :);        
        tforms(i) = estimateGeometricTransform2D(matchedPoints, matchedPointsPrev,"similarity",...
                                                 "Confidence" , params.Confidence, ...
                                                 "MaxDistance", params.MaxDistance, ...
                                                 "MaxNumTrials",params.MaxNumTrials);
        
        % Compute the transformation that maps images{i} to the stitched
        % image as T(i)*T(i-1)*...*T(1).
        tforms(i).T = tforms(i).T*tforms(i-1).T;
    end
end

function [outputImage, outputView] = helperStitchImages(images, tforms) 
    
    numImages = numel(images);
    imageSize = zeros(numImages,2);
    xlim = zeros(numImages,2);
    ylim = zeros(numImages,2);
    
    % Compute the output limits for each transform.
    for i = 1:numel(images)
        imageSize(i,:) = size(images{i}, 1:2);
        [xlim(i,:), ylim(i,:)] = outputLimits(tforms(i), ...
                                              [1 imageSize(i,2)], ...
                                              [1 imageSize(i,1)]);
    end    

    % Find the minimum and maximum output limits.
    maxImageSize = max(imageSize);
    
    xMin = min([1; xlim(:)]);
    xMax = max([maxImageSize(2); xlim(:)]);

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

    % Width and height of panorama.
    width  = round(xMax - xMin);
    height = round(yMax - yMin);

    % Initialize the "empty" panorama.
    outputImage = zeros([height width 3], "like", images{1});

    % Create a 2-D spatial reference object defining the size of the panorama.
    xLimits = [xMin xMax];
    yLimits = [yMin yMax];
    outputView = imref2d([height width], xLimits, yLimits);

    % Step 7 - Stitch the images.
    for i = 1:numel(tforms)
        % Apply transformation.
        warpedImage = imwarp(images{i}, tforms(i), "OutputView", outputView);
        
        % Blend the images.
        outputImage = helperBlendImages(warpedImage, outputImage);
    end
end

function tranformedImagePoints = helperVehicleToBirdsEyeView(birdsEye, vehiclePoints, Rout)

    % Transform the 3D worldPoints in vehicle coordinate system to 2D
    % imagePoints in the Bird's-Eye-View (BEV) image.
    imagePoints = vehicleToImage(birdsEye, vehiclePoints);
    
    % Transform these imagePoints from single BEV image to 360° bird's-eye-view images.
    [xPoints, yPoints] = worldToIntrinsic(Rout{1}, imagePoints(:,1), imagePoints(:,2));
    [xPoints, yPoints] = worldToIntrinsic(Rout{2}, xPoints, yPoints);
    
    tranformedImagePoints = [xPoints, yPoints];
end

function [x, y, h, w] = helperGetImageBoundaryOnBEV(birdsEye, distFromVehicle, Rout)
    
    % Define three corner points of the vehicle's bounding box.
    vehiclePoints = [ distFromVehicle, distFromVehicle;
                     -distFromVehicle, distFromVehicle; 
                     -distFromVehicle,-distFromVehicle];
    
    % Flip the x and y axes between the world and vehicle coordinate
    % systems.
    vehiclePoints = fliplr(vehiclePoints);
    
    imagePoints = helperVehicleToBirdsEyeView(birdsEye, vehiclePoints, Rout);
    x = imagePoints(1,1);
    y = imagePoints(1,2);
    h = abs(imagePoints(1,1) - imagePoints(2,1));
    w = abs(imagePoints(2,2) - imagePoints(3,2));
end

function [polygonPoints, vehicleLength, vehicleWidth] = ...
          helperGetVehicleBoundaryOnBEV(birdsEye, vehicleCenter, vehicleSize, Rout)

    x = vehicleCenter(1);
    y = vehicleCenter(2);

    % Find half length and half width in a and b respectively.
    a = vehicleSize(1)/2;
    b = vehicleSize(2)/2;
    
    % Define vehiclePoints in the world coordinate system.    
    vehiclePoints = [ x+b, y+a;
                      x+b, y-a;
                      x-b, y-a;
                      x-b, y+a ];
    
    % Flip the x and y axes between the world and vehicle coordinate
    % systems.
    vehiclePoints      = fliplr(vehiclePoints);
    imagePoints = helperVehicleToBirdsEyeView(birdsEye, vehiclePoints, Rout);
    
    xPoints = ceil(imagePoints(:,1));
    yPoints = ceil(imagePoints(:,2));
    
    vehicleLength = abs(yPoints(1) - yPoints(2));
    vehicleWidth  = abs(xPoints(2) - xPoints(3));
    
    % x,y points as points on a polygon for showShape.
    polygonPoints = [xPoints   , yPoints;...
    % Close the polygon by setting the last point as first point.
                     xPoints(1), yPoints(1)]; 
end

function outputImage = helperOverlayImage(topImage, bottomImage) 
    
    blender = vision.AlphaBlender("Operation", "Binary mask", ...
              "MaskSource", "Input port");    
    mask = sum(topImage, 3) ~= 0;    
    outputImage = step(blender, bottomImage, topImage, mask);
end