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