%% Exercise "Generating a point cloud from stereo images"
%
% 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

%% Calibrating the Camera
% First of all you need to calibrate the camera. Therefore you will need a
% printout of the Checkerboard Pattern which is part of every Matlab
% installation ("open checkerboardPattern.pdf"). The size of the squares
% may vary, depending on your printer and its settings, so you have to
% measure the actual size on the printout. To acquire the images needed for
% the calibration you can use the Camera Calibrator app (cameraCalibrator).
% For good results you should take 10 or more images with the entire
% checkerboard visible. The acquisition can be done within the app itelf.
% Check your results and remove eventual outliers.
cameraCalibrator

%% 
% You need to export the created calibration file and save it for later 
% use. Normally the calibration is stored in the 'cameraParams' which you
% just need so save as a Matlab file, because otherwise you would have
% to calibrate the camera every time.
save('calibration.mat', 'cameraParams')

%% 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.
clear, clc, close all
mylego = legoev3('USB');
cam = webcam('C922 Pro Stream Webcam');
mymotor_A = motor(mylego,'A');

%% 
%  Because you need a large amount of overlap in the images (>60%) we only
%  move the vehicle a few centimeters. Because we're mounting the camera on
%  top of the lego vehicle, which is on tracks, we won't need a tripod. We
%  take a picture of the object, move the vehicle for 0.8 seconds with 60
%  speed, take another picture and move back to the starting location. To
%  make sure that the vehicle comes to a stop and the camera has time to
%  autofocus, we included some pauses.
mymotor_A.Speed = 60;
I1 = snapshot(cam);
pause(0.5)
tic
while toc <= 0.8
      start(mymotor_A)
end
stop(mymotor_A);
pause(1)
I2 = snapshot(cam);
pause(1)

% Return to start
tic
mymotor_A.Speed = -60;
while toc <= 0.8
      start(mymotor_A)
end
stop(mymotor_A); 

% Save the images.
imwrite(I1,'Image_1.png')
imwrite(I2,'image_2.png')

%% Calculate 3D point cloud using stereo images
% Start here if you have acquired the images stored in Image_1.png and in
% Image_2.png. Load the images and the camera parameters and undistort the
% image
clear, clc, close all
load calibration.mat

I1 = imread('Image_1.png');
I2 = imread('Image_2.png');

I1 = undistortImage(I1, cameraParams);
I2 = undistortImage(I2, cameraParams);

%%
% Detect corners and their position, which can be found in both images. For
% the algorithm to work, it has to be converted to a grayscale image.
pointsI1 = detectMinEigenFeatures(rgb2gray(I1));
tracker = vision.PointTracker('MaxBidirectionalError', 1, ...
    'NumPyramidLevels', 5);

% Extract the location of those points and start the tracking process
pointsI1 = pointsI1.Location;
initialize(tracker, pointsI1, I1);

% Track the points
[pointsI2, trackID] = step(tracker, I2);
matchedI1 = pointsI1(trackID, :);
matchedI2 = pointsI2(trackID, :);

%%
% Estimating the essential Matrix
[eMatrix, epipolarInliers] = estimateEssentialMatrix(matchedI1, ...
    matchedI2, cameraParams, 'Confidence', 50);

%%
% Find epipolar inliers
inliersI1 = matchedI1(epipolarInliers, :);
inliersI2 = matchedI2(epipolarInliers, :);

%%
% Calculate the position, rotation and translataion between the two images
% using the calibration file.
[R,t] = cameraPose(eMatrix, cameraParams, inliersI1, inliersI2);

% Camera projection matrix
matrixI1 = cameraMatrix(cameraParams, eye(3), [0 0 0]);
matrixI2 = cameraMatrix(cameraParams, R', -t*R');

% Calculate the 3D-Points
points3D = triangulate(matchedI1, matchedI2, matrixI1, matrixI2);

% Create the point cloud
ptCloud = pointCloud(points3D);

% Save the results
save point_cloud.mat

%%
% Visualizing the point cloud
close all
figure1 = figure(...
    'Position',[200 200 800 400],...
    'Color',[1 1 1]);
axes1 = axes(...
    'Box','on',...
    'LineWidth',0.6,...
    'FontName','Helvetica',...
    'FontSize',12,...
    'XGrid','On',...
    'YGrid','On',...
    'ZGrid','On'); hold on
pcshow(ptCloud,...
     'MarkerSize',78)
xlabel('x-axis');
ylabel('y-axis');
zlabel('z-axis');
view(-30,30)

print -dpng -r300 Exercises_Generating_a_point_cloud_from_stereo_images.png








