%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% perform a multi-camera stitched calibration
% 
% camera intrinsic parameters are estimated first
% then pair-wise estimation is made of the relative pose 
% 

addpath(genpath([pwd '/../core/']));

%% input parameters
calib_dir       = 'L:\array\tcf\6C_A\calib\';
calib_pat       = '%s/CAM%d-%02d.mat';
setup_pat       = '%s/camera-model.mat';
calib_file_fun=@(c,t) sprintf(calib_pat, calib_dir, c, t);
n_views         = 21; % number of different positions of calibration targets
n_cameras       = 6;  % number of cameras

b_estimate_skew     = false;

%% get handle to computer vision toolbox triangulate function
% this is shadowed elsewhere by triangulate
tbox_dir = [toolboxdir('vision') '/vision'];
cdir     = pwd;
cd(tbox_dir);
cvtoolbox_triangulate= @triangulate;
cd(cdir);

%% load data from first target
cfile       = calib_file_fun(1,1);
fprintf('load example %s\n', cfile);
ds          = load(cfile);
% extract the details of the chessboard grid
u_lim       = ds.target.u_lim;
v_lim       = ds.target.v_lim;
cb_spacing  = ds.target.grid_spacing;
cb_dimension= ds.target.grid_dimension;
cb_units    = ds.target.grid_units;
% image simze
im_size     = size(ds.source_image);
i_axis      = ds.i_axis;
j_axis      = ds.j_axis;

% report 
fprintf('chessboard target:\n');
fprintf('   %d x %d squares (%0.1f %s side)\n', cb_dimension, cb_spacing, cb_units);
fprintf('   %0.1f x %0.1f %s\n', cb_dimension * cb_spacing, cb_units);

%% define the maximum observable set of chessboard corners
% with M x N squares, we can see (M-1) x (N-1) corners
n_cb_corners= prod(cb_dimension-1); 
% these span
u_corners   = u_lim(1) + cb_spacing : cb_spacing : u_lim(2) - cb_spacing;
v_corners   = v_lim(1) + cb_spacing : cb_spacing : v_lim(2) - cb_spacing;
[ucorn,vcorn]=ndgrid(u_corners, v_corners);
uv_corners  = [ucorn(:) vcorn(:)].';

%% initialise camera setup structure
setup           = struct();
setup.n_cameras = n_cameras;
setup.n_views   = n_views;
setup.camera    = cell(1,n_cameras);% convert to structure array later
setup.delta_t   = 1;                % leave as 1 for now
% compute adjacency matrix and reconstruction limits later

%% load planar calibration targets
im_corners_ary  = cell(1,n_cameras);
for c = 1 : n_cameras
    %% allocate arrays for partial data 
    img_points  = nan(n_cb_corners, 2, n_views);

    %% load data for each view of the target
    viewdata    =  struct('uv_markers', zeros(3,0), ...
                          'im_markers', zeros(2,0), ...
                          'id', 1, ...
                          'Hrel', eye(4));
    viewdata(1:n_views) = viewdata;
    for v = 1 : n_views
        %% load file
        cfile   = calib_file_fun(c,v);
        fprintf('camera %d, view %d: %s\n', c, v, cfile);
        ds      = load(cfile);

        %% get marker image and plate coordinates 
        % for camera calibration purposes with MATLAB inbuilt routines
        % it is necessary to convert from
        % the local coordinate system of the camera to the row/column
        % coordinate system
        uv_mark = ds.target.uv_markers;
        ij0     = [ds.i_axis(1); ds.j_axis(1)];
        im_mark = ds.target.im_markers - ij0 + 1;
        if isempty(uv_mark)
            %warning('no markers, skipping');
            continue;
        end

        %% find index in maximum set
        [ind,dist]= knnsearch(uv_corners.', uv_mark.', 'K', 1);
        toofar  = dist > 0.1 * cb_spacing;
        assert(~any(toofar), 'there were markers we could not identify correctly');

        %% store in array
        img_points(ind, :, v) = im_mark.';
        viewdata(v).uv_markers= [uv_mark;zeros(1,size(uv_mark,2))];% add bottom row of zeros
        viewdata(v).im_markers= im_mark;
        viewdata(v).id        = v;
    end

    %% initialise camera model
    cmodel       = struct();
    cmodel.id    = ds.id;    % camera ID
    cmodel.i_axis= 1 : im_size(1);
    cmodel.j_axis= 1 : im_size(2);
    cmodel.view  = viewdata;
    cmodel.calib = []; % empty structure for calibration

    %% store in structure
    im_corners_ary{c} = img_points;
    setup.camera{c}   = cmodel;
end

%% estimate intrinsic camera parameters
fprintf('------------------------------------------------------------------\n');
fprintf('estimating intrinsic camera parameters\n');
cparams_ary = cell(1,n_cameras);
for c = 1 : n_cameras
    %% estimate
    fprintf('camera %d\n', c);
    [cparams_ary{c},ivalid] = ...
        estimateCameraParameters(im_corners_ary{c}, uv_corners.', ...
        'ImageSize', im_size, ...
        'NumRadialDistortionCoefficients', 2, ...
        'EstimateTangentialDistortion', false, ...
        'EstimateSkew', b_estimate_skew);
    %% update view data
    % include only valid views
    %{
    ivalid=find(ivalid);
    setup.camera{c}.view = setup.camera{c}.view(ivalid);
    n_valid_view = nnz(ivalid);
    for v = 1 : n_valid_view
        % target position/orientation relative to camera
        setup.camera{c}.view(v).Hrel = cparams_ary{c}.PatternExtrinsics(v).A;
    end
    %}
end

%% estimate the relative pose camera pairs
% based on point correspondences across multiple images
c_pairs     = [1:n_cameras-1; 2:n_cameras].';
n_pairs     = size(c_pairs, 1);
rel_pose_ary= cell(1, n_pairs);

for p = 1 : n_pairs
    %% reportt
    fprintf('finding fundamental matrix for C%d-C%d\n', c_pairs(p,:));

    %% find a set of point correspondences
    c1              = c_pairs(p,1);
    c2              = c_pairs(p,2);
    % permute so component is first dimension
    im_corners_c1   = permute(im_corners_ary{c1}, [2 1 3]);
    im_corners_c2   = permute(im_corners_ary{c2}, [2 1 3]);
    mutual          = ~isnan(im_corners_c1(1,:,:)) & ~isnan(im_corners_c2(1,:,:));
    % select only mutually visible
    im_corners_c1   = im_corners_c1(:,mutual);
    im_corners_c2   = im_corners_c2(:,mutual);
    n_mutual        = nnz(mutual);

    %% estimate essential matrix
    [E12,inliers,status] = estimateEssentialMatrix(im_corners_c1.', im_corners_c2.', cparams_ary{c1}, cparams_ary{c2});
    fprintf('%d points available for correspondence matching\n', nnz(mutual));
    fprintf('%d inliers chosen\n', nnz(inliers));

    %% obtain relative pose from essential matrix
    % this can be defined up to a scale factor
    relativePose    = estrelpose(E12, ...
        cparams_ary{c1}.Intrinsics, cparams_ary{c2}.Intrinsics, ...
        im_corners_c1(:,inliers).', im_corners_c2(:,inliers).');
    relativeOrientation = relativePose.R;
    relativeLocation= relativePose.Translation;
    camPose         = rigidtform3d(relativeOrientation,relativeLocation);
    extrinsics      = pose2extr(camPose);
    % compute normalised projection matrices
    P1_norm         = [eye(3) zeros(3,1)];
    P2_norm         = [extrinsics.R extrinsics.Translation'];
    % apply intrinsics to get projection matrix
    P1              = cparams_ary{c1}.Intrinsics.K * P1_norm;
    P2              = cparams_ary{c2}.Intrinsics.K * P2_norm;
    
    %% determine the unknown scale factor
    % we know the relative distance between points on the calibration
    % target. if we reproject with our (unscaled) calibration model, we can
    % compare the relative distances in the reprojection to ground-truth
    % in order to obtain the unknown scale factor
    % 
    % it is also possible to factor in the relative translation of the
    % calibration target
    in_plane_pairs  = cell(1, n_views);
    uv_distance     = []; % distance between points on calibration target
    X_distance      = []; % distance between points in reprojection
    for plane = 1 : n_views
        %% find pairs of mutually visible points in this plane
        ind         = find(mutual(1,:,plane)); 
        im_c1       = im_corners_ary{c1}(ind,:,plane).';
        im_c2       = im_corners_ary{c2}(ind,:,plane).';
        n_mutual_plane    = numel(ind);
        if n_mutual_plane < 1
            continue;
        end
        uv_mutual    = uv_corners(:, ind); % coordinates on planar target

        %% evaluate distance between all pairs of mutually visible points in this plane
        X_mutual     = cvtoolbox_triangulate(im_c1.', im_c2.', P1, P2).';
        [k,l]       = ndgrid(1:n_mutual_plane, 1:n_mutual_plane);
        Delta_uv    = uv_mutual(:,k) - uv_mutual(:,l);
        Delta_X     =  X_mutual(:,k)  - X_mutual(:,l);
        uv_distance = [uv_distance, sqrt(sum(Delta_uv.^2,1))]; %#ok<AGROW> 
        X_distance  = [X_distance , sqrt(sum(Delta_X.^2 ,1))]; %#ok<AGROW> 
    end
    % exclude pair correspondences with zero distance
    zerodist        = uv_distance == 0;
    uv_distance(zerodist)=[];
    X_distance(zerodist) =[];
    % solve in least squares sense for unkown scale factor
    scalefactor     = uv_distance * pinv(X_distance);
    
    %% store result in structure
    pair            = struct();
    pair.cidx       = c_pairs(p,:); 
    pair.E12        = E12;           % essential matrix
    % rotation and translation of camera 1
    pair.R1         = eye(3);
    pair.t1         = zeros(3,1);
    % rotation and translation of camera 2
    pair.R2         = extrinsics.R;
    pair.t2         = extrinsics.Translation.' * scalefactor;

    rel_pose_ary{p} = pair;

    continue;
    %% check: does reprojection makes sense?
    % apply scale factor to camera matrices
    P0_scaled       = P1 * diag([1 1 1 scalefactor]);
    P1_scaled       = P2 * diag([1 1 1 scalefactor]);

    X_mutual        = cvtoolbox_triangulate(im_corners_c1.', im_corners_c2.', P0_scaled, P1_scaled).';
    imt_c1          = P0_scaled * [X_mutual; ones(1, n_mutual)];
    imt_c2          = P1_scaled * [X_mutual; ones(1, n_mutual)];
    im_c1           = imt_c1 ./ imt_c1(3,:);
    im_c2           = imt_c2 ./ imt_c2(3,:);
    figure(1);
    clf;
    subplot(1,2,1);
    plot(im_corners_c1(1,:),im_corners_c1(2,:),'ro');
    hold on;
    plot(im_c1(1,:),im_c1(2,:),'b.');
    subplot(1,2,2);
    plot(im_corners_c2(1,:),im_corners_c2(2,:),'ro');
    hold on;
    plot(im_c2(1,:),im_c2(2,:),'b.');
end

%% obtain relative camera poses by induction
H1n             = eye(4);
Hn              = repmat(eye(4), [1 1 n_cameras]);
for p = 1 : n_pairs
    %% obtain relative pose with respect to previous camera
    H12     = [rel_pose_ary{p}.R2, rel_pose_ary{p}.t2; 0 0 0 1];
    H1n     = H1n * H12;
    Hn(:,:,p+1)=H1n;
end

%% convert MATLAB calibration to our calibration format
for c = 1 : n_cameras
    %% create camera calibration structure
    calib   = struct();
    intrinsics=cparams_ary{c}.Intrinsics;
    H       = Hn(:,:,c);
    calib.G = intrinsics.K;             % intrinsic parameters
    calib.R = H(1:3,1:3);               % rotation matrix
    calib.xc= -H(1:3,4);                % camera position
    calib.P = [calib.G zeros(3,1)] * H; % projection matirx
    calib.kr= [0 0]; % intrinsics.RadialDistortion(:);
    calib.ic= intrinsics.PrincipalPoint(:);
    setup.camera{c}.calib = calib;
    % use BGGR bayer filter 
    setup.camera{c}.bayer = cat(3, [1 0; 0 0], [0 1; 1 0], [0 0; 0 1]);
end
setup.camera = cell2mat(setup.camera);

%% evaluate reconstruction limits and adjacency matrix
% additional constraints on bounding box come from limits of where markers
% have been recognised. this avoids extrapolating 
fprintf('building reconstruction limits...\n')
[setup.re_bb_lim,setup.re_A,setup.re_b]= bounding_box3(setup.camera, [], []);
% build adjacency matrix
setup.adjacency = build_adjacency_matrix(setup.camera, setup.re_bb_lim);

%% report size of measurable volume
fprintf('reconstruction limits are:\n');
fprintf('  x = %+6.1f - %+6.1f mm\n', setup.re_bb_lim(:,1));
fprintf('  y = %+6.1f - %+6.1f mm\n', setup.re_bb_lim(:,2));
fprintf('  z = %+6.1f - %+6.1f mm\n', setup.re_bb_lim(:,3));

%% save to file
setup_file      = sprintf(setup_pat, calib_dir);
fprintf('saving to %s\n', setup_file);
save(setup_file, '-struct', 'setup');
fprintf('done\n');

%% plot poses of cameras
figure(2);
clf;
hold on;
for c = 1 : n_cameras
    extr      = rigidtform3d(Hn(:,:,c));
    camPose   = extr2pose(extr);
    plotCamera(AbsolutePose=camPose, Size=20);
end
axis equal xy tight;
view(3);

%% show estimate of extrinsic camera parameters
figure(1);
clf;
for c = 1 : n_cameras
    %% show
    subplot(1,n_cameras,c);
    cla;
    showExtrinsics(cparams_ary{c});
    view(0,90);
    title(sprintf('camera %d',c));
    xlim([-200 200]);
end