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