+Calibration/compareRealData.m
function hlink = compareRealData(reconId, calTypes, algNames, calibInfo)
%% Compare reconstructed point cloud with Intel RealSense F200 captured point cloud.
% Note that for better reconstruction comparisons, we use dense multi-shot
% SL pixe mappings from MT's method, not our sparse SL pixel mapping.
% Only the calibration parameters are different for the compared methods.

%% License
% ACADEMIC OR NON-PROFIT ORGANIZATION NONCOMMERCIAL RESEARCH USE ONLY
% Copyright (c) 2018 Bingyao Huang
% All rights reserved.

% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met: 

% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.

% If you publish results obtained using this software, please cite our paper.

% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
% SOFTWARE.

%%
disp(' ');
disp('Comparing reconstrcuted 3D object with depth camera captured 3D object, please wait...')
%% Load realsense data
% reconId = '12';
realSenseReconName = fullfile(calibInfo.path, 'realSenseRecon', ['recon-', reconId, '.ply']);
calibReconName = fullfile(calibInfo.path, 'MT', ['Set', sprintf('%02d', str2num(reconId))], 'sl.yml');

% RealSense reconstructed point cloud
if(exist(realSenseReconName, 'file'))
    pcRS = pcread(realSenseReconName);
else
    error(['File ', realSenseReconName, ' does not exist!'])
end

%% Load calibration data and SL point pairs
alignErr = zeros(size(calTypes, 1), 5);

% cam and prj SL point pairs generated by Moreno & Taubin software
if(exist(calibReconName, 'file'))
    slPtsPair = cv.FileStorage(calibReconName);
else
    error(['File ', calibReconNamem, ' does not exist!'])
end

pcCalib = [];
for i=1:length(calTypes)
    curCalType = calTypes{i};
    param = cv.FileStorage(fullfile(calibInfo.resultDir, [curCalType, '.yml']));
    
    % reconstruct
    pcCalib{i} = triangulatePointPairs(param, slPtsPair);
    vecErr{i} = Reconstruct.alignmentError( pcCalib{i}, pcRS, param, calibInfo.camW, calibInfo.camH, 1, algNames{i});
    
    % compute metrics
    rmse = vecErr{i};
    alignErr(i,1) = min(rmse);
    alignErr(i,2) = max(rmse);
    alignErr(i,3) = mean(rmse);
    alignErr(i,4) = median(rmse);
    alignErr(i,5) = std(rmse);
   
    ax(i) = gca;
end

% link the plots
% hlink = linkprop(ax,{'CameraPosition','CameraUpVector', 'xlim', 'ylim', 'zlim'});
hlink = linkprop(ax,{'CameraPosition','CameraUpVector'});

%% Display results in table

% 3d alignment error
metricsTable = array2table(alignErr,...
    'VariableNames',{'min','max','mean', 'median','std'},...
    'RowNames', algNames);
disp('3D reconstructed and RealSense captured point cloud 3D alignment error:'); 
disp(metricsTable);


%% Export figures
% save figures
SAVE_FIG = 0;

n = 1;
figIds = findobj('Type', 'figure');
% sort according to number
[val, idx] = sort([figIds.Number]);
figIds = figIds(idx);

if(SAVE_FIG)

    % export as png format
    exportPath = 'doc\recon\';
    for i=1:length(figIds)
        curFig = figure(figIds(i));
        figName = strrep(curFig.Name, '/', ' ');
        figName = [num2str(n),  '_', strrep(figName, ' ', '_')];
        savefig(curFig, [exportPath, figName, '.fig']);
        export_fig(curFig, [exportPath, figName, '.png'], '-transparent'); % you need to install export_fig package
        n = n + 1;
    end
end

end

%% Compute point cloud using calibrated params and 2d point pairs
function [ptCloud] = triangulatePointPairs(param, ptsYML)

% undistort points
camPtsImg = ImgProc.cvUndistortPoints(ptsYML.camPts2d, param.camK, param.camKc);
prjPtsImg = ImgProc.cvUndistortPoints(ptsYML.prjPts2d, param.prjK, param.prjKc);

% triangulate
[ptCloud, reprjErr] = Reconstruct.pointCloudFromPts2d(camPtsImg, prjPtsImg, param.camK, param.prjK, param.R, param.T);

end
