Subversion Repositories seema-scanner

function MSE = alignSubScansMarkers(calibrationFileName, scanDir, alnFileName)

%ALIGNSUBSCANSMARKERS Determines an exact alignment of sub scans (scans

% from e.g. one revolution of the rotation stage). 

% The method searches for circular white markers of a specific diameter.

% White frames corresponding to each sub scan must be available.

% A coarse alignment in the form of an aln-file must be provided. 

%

% 2017 Jakob Wilm, DTU

initialAlign = readMeshLabALN(alnFileName);

whiteFrameDirs = dir(fullfile(scanDir, 'sequence_*'));

assert(length(whiteFrameDirs) == length(initialAlign));

calibration = readOpenCVXML(calibrationFileName);

% full projection matrices in Matlab convention

P0 = transpose(calibration.K0*[eye(3) zeros(3,1)]);

P1 = transpose(calibration.K1*[calibration.R1 calibration.T1']);

% matlab cam params for undistortion

camParams0 = cameraParameters('IntrinsicMatrix', calibration.K0', 'RadialDistortion', calibration.k0([1 2 5]), 'TangentialDistortion', calibration.k0([3 4]));

camParams1 = cameraParameters('IntrinsicMatrix', calibration.K1', 'RadialDistortion', calibration.k1([1 2 5]), 'TangentialDistortion', calibration.k1([3 4]));

% matlab struct for triangulation

camStereoParams = stereoParameters(camParams0, camParams1, calibration.R1', calibration.T1');

nSubScans = length(whiteFrameDirs);

% ellipse detection settings

ep = struct('minMajorAxis', 25, 'maxMajorAxis', 30, 'minAspectRatio', 0.4, 'randomize', 0, 'smoothStddev', 2);

% 3D coordinates of markers in local camera frame

E = cell(nSubScans, 1);

% 3D coordinates of markers in global initial alignment

Eg = cell(size(E));

% find 3D markers coordinates 

for i=1:nSubScans

    % load point cloud

    pc = pcread(fullfile(scanDir, initialAlign(i).FileName));

    Q = pc.Location;

    idString = initialAlign(i).FileName(12:end-4);

    % load white frames

    frame0 = imread(fullfile(scanDir, ['sequence_' idString], 'frames0_0.png'));

    frame1 = imread(fullfile(scanDir, ['sequence_' idString], 'frames1_0.png'));

    %e0Coords = autoDetectMarkers(frame0, ep);

    %e1Coords = autoDetectMarkers(frame1, ep);

    e0Coords = manuallyDetectMarkers(frame0, ep, P0, Q);

    e1Coords = manuallyDetectMarkers(frame1, ep, P1, Q);

    e0Coords = undistortPoints(e0Coords, camParams0);

    e1Coords = undistortPoints(e1Coords, camParams1);

    % match ellipse candidates between cameras based on projection

    E0 = projectOntoPointCloud(e0Coords, P0, Q);

    E1 = projectOntoPointCloud(e1Coords, P1, Q);

    matchedPairs = nan(size(E0, 1), 2);

    nMatchedPairs = 0;

    for j=1:size(E0, 1)

        % should use pdist2 instead..

        sqDists = sum((E1 - repmat(E0(j,:), size(E1, 1), 1)).^2, 2);

        [minSqDist, minSqDistIdx] = min(sqDists);

        if(minSqDist < 5^2)

            nMatchedPairs = nMatchedPairs + 1;

            matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];

        end

    end

    matchedPairs = matchedPairs(1:nMatchedPairs, :);

    % triangulate marker centers (lens correction has been performed)

    E{i} = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);

    % bring into initial alignment

    [U,~,V] = svd(initialAlign(i).Rotation);

    Ri = U*V';

    Ti = initialAlign(i).Translation;

    Eg{i} = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);

end

% show found markers in initial alignment

figure;

hold('on');

for i=1:nSubScans

    % fix Ri to be orthogonal

    [U,~,V] = svd(initialAlign(i).Rotation);

    Ri = U*V';

    % bring point cloud into initial alignment

    pc = pcread(fullfile(scanDir, initialAlign(i).FileName));

    tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);

    pcg = pctransform(pc, tform);

    pcshow(pcg);

    xlabel('x');

    ylabel('y');

    zlabel('z');

    plot3(Eg{i}(:,1), Eg{i}(:,2), Eg{i}(:,3), '.', 'MarkerSize', 15);

end

% match markers between poses using initial alignment

Pg = {};

P = {};

for i=1:nSubScans

    for j=1:size(Eg{i}, 1)

        pg = Eg{i}(j,:);

        p = E{i}(j,:);

        matched = false;

        for k=1:size(Pg, 2)

            clusterCenter = mean(cat(1, Pg{:,k}), 1);

            if(sum((pg - clusterCenter).^2) < 3^2)

                % store in global frame

                Pg{i,k} = pg;

                % store in local frame

                P{i,k} = p;

                matched = true;

                break;

            end

        end

        % create new cluster

        if(not(matched))

            Pg{i,end+1} = pg;

            P{i,end+1} = p;

        end 

    end

end

% run optimization

alignment = groupwiseOrthogonalProcrustes(P, initialAlign);

for i=1:length(alignment)

    alignment(i).FileName = initialAlign(i).FileName;

end

writeMeshLabALN(alignment, strrep(alnFileName, '.aln', '_opt.aln'));

end

function e = autoDetectMarkers(frame, ep)

    % create mask based on morphology

    bw = imbinarize(rgb2gray(frame));

    cc = bwconncomp(bw);

    rp = regionprops(cc, 'Area', 'Solidity', 'Eccentricity', 'Centroid');

    idx = find([rp.Area] > 100 & [rp.Area] < 1000 & [rp.Solidity] > 0.9);

    mask = ismember(labelmatrix(cc), idx);

    mask = imdilate(mask, strel('disk', 20, 0));

    % detect ellipses within mask

    edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);

    edges(~mask) = 0;

    ep.numBest = 10; 

    el = ellipseDetection(edges, ep);

    e = el(:, 1:2);

end

function e = manuallyDetectMarkers(frame, ep, P, pointCloud)

    e = [];

	edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);

    figure; 

    hold('on');

    imshow(frame);

    title('Close figure to end.');

    set(gcf, 'pointer', 'crosshair'); 

    set(gcf, 'WindowButtonDownFcn', @clickCallback);

    uiwait;

    function clickCallback(~, ~)

        p = get(gca, 'CurrentPoint'); 

        p = p(1, 1:2);

%         % create mask around selected point

%         mask = false(size(frame, 1), size(frame, 2));

% 

%         mask(round(p(2)), round(p(1))) = true;

%         mask = imdilate(mask, strel('disk', 100, 0));

% 

%         % detect ellipses within mask

%         edgesI = edges;

%         edgesI(~mask) = 0;

% 

%         ep.numBest = 1; 

%         el = ellipseDetection(edgesI, ep);

% 

%         ellipse(el(:,3), el(:,4), el(:,5)*pi/180, el(:,1), el(:,2), 'r'); 

%         

%         e = [e; el(:, 1:2)];

        [el, conf] = detectMarkersSubpix(frame, p, P, pointCloud)

        e = [e; el(:, 1:2)];

    end

end

function [e, conf] = detectMarkersSubpix(frame, initGuesses, P, Q)

    % create mask based on morphology

    bw = imbinarize(rgb2gray(frame));

    cc = bwconncomp(bw);

    labels = labelmatrix(cc);

    % project point cloud into image

    q = [Q ones(size(Q,1),1)]*P;

    q = q./[q(:,3) q(:,3) q(:,3)];

    for i=1:size(initGuesses, 1)

        labelId = labels(round(initGuesses(i,2)), round(initGuesses(i,1)));

        labelMask = (labels == labelId);

        labelMask = imdilate(labelMask, strel('disk', 3, 0));

        % determine 3D points that are part of the marker

        pointMask = false(size(q, 1), 1);

        for j=1:size(q,1)

            if(labelMask(round(q(j,2)), round(q(j,1))))

                pointMask(j) = true;

            end

        end

        % build homography

        H = fitgeotrans(Q(pointMask, :), q(pointMask, :), 'projective');

    end

    e = initGuesses;

    conf = 1.0;

end

function E = projectOntoPointCloud(e, P, pointCloud)

    q = [pointCloud ones(size(pointCloud,1),1)]*P;

    q = q(:,1:2)./[q(:,3) q(:,3)];

    E = nan(size(e,1), 3);

    for i=1:size(e, 1)

        sqDists = sum((q - repmat(e(i,:), size(q, 1), 1)).^2, 2);

        [minSqDist, minSqDistIdx] = min(sqDists);

        if(minSqDist < 2^2)

            E(i, :) = pointCloud(minSqDistIdx, :);

        end

    end    

end