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 the specified 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 = autoDetectEllipses(frame0, ep);

%     e1Coords = autoDetectEllipses(frame1, ep);

    e0Coords = manuallyDetectEllipses(frame0, ep);

    e1Coords = manuallyDetectEllipses(frame1, ep);

    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

% match markers between poses using initial alignment

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);

    figure;

    hold('on');

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

    pcshow(pcg);

    xlabel('x');

    ylabel('y');

    zlabel('z');

end

end

function e = autoDetectEllipses(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(cc0), 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; 

    e = ellipseDetection(edges, ep);

end

function e = manuallyDetectEllipses(frame, ep)

    figure; 

    imshow(frame);

    title('Press return to end');

    p = ginput();

    e = [];

    for i=1:size(p, 1)

        % create mask around selected point

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

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

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

        % detect ellipses within mask

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

        edges(~mask) = 0;

        ep.numBest = 1; 

        el = ellipseDetection(edges, ep);

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

    end

    close(gcf);

end

function E = projectOntoPointCloud(e, P, Q)

    q = [Q ones(size(Q,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, :) = Q(minSqDistIdx, :);

        end

    end    

end