Subversion Repositories seema-scanner

function MSE = alignSubScansMarkers(calibrationFileName, 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);

[alnFilePath, ~, ~] = fileparts(alnFileName);

calibration = readOpenCVXML(calibrationFileName);

% correct for Matlab 1-indexing in principle point coordinates

calibration.K0(1:2, 3) = calibration.K0(1:2, 3)+1;

calibration.K1(1:2, 3) = calibration.K1(1:2, 3)+1;

% 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(initialAlign);

%nSubScans = 5;

% 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

    pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);

    pcFilePath = fileparts(pcFileName);

    pc = pcread(pcFileName);

    Q = pc.Location;

    idString = strsplit(initialAlign(i).FileName, {'.ply', '_'});

    idString = idString{end-1};

    % load white frames

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

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

    e0Coords = autoDetectMarkers(frame0);

    e1Coords = autoDetectMarkers(frame1);

    %e0Coords = manuallyDetectMarkers(frame0);

    %e1Coords = manuallyDetectMarkers(frame1);

    %[e0Coords, conf0] = detectMarkersSubpix(frame0, e0Coords, P0, Q);

    %[e1Coords, conf1] = detectMarkersSubpix(frame1, e1Coords, P1, Q);

    if(length(e0Coords) < 1 || length(e1Coords) < 1)

        continue;

    end

%     figure; 

%     subplot(1,2,1);

%     imshow(frame0);

%     hold('on');

%     plot(e0Coords(:,1), e0Coords(:,2), 'rx', 'MarkerSize', 15);

%     subplot(1,2,2);

%     imshow(frame1);

%     hold('on');

%     plot(e1Coords(:,1), e1Coords(:,2), 'rx', 'MarkerSize', 15);

%     drawnow;

    e0CoordsUndistort = undistortPointsFast(e0Coords, camParams0);

    e1CoordsUndistort = undistortPointsFast(e1Coords, camParams1);

    % match ellipse candidates between cameras based on projection

    E0 = projectOntoPointCloud(e0CoordsUndistort, P0, Q);

    E1 = projectOntoPointCloud(e1CoordsUndistort, 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 < 2^2)

            nMatchedPairs = nMatchedPairs + 1;

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

        end

    end

    matchedPairs = matchedPairs(1:nMatchedPairs, :);

    figure; 

    subplot(1,2,1);

    imshow(frame0);

    hold('on');

    plot(e0Coords(matchedPairs(:, 1),1), e0Coords(matchedPairs(:, 1),2), 'rx', 'MarkerSize', 15);

    subplot(1,2,2);

    imshow(frame1);

    hold('on');

    plot(e1Coords(matchedPairs(:, 2),1), e1Coords(matchedPairs(:, 2),2), 'rx', 'MarkerSize', 15);

    drawnow;

%     % triangulate marker centers (lens correction has been performed)

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

%     E{i} = E{i}(e<3.0, :);

%     display(e);

    [E{i}, e] = detectMarkersStereoSubpix(frame0, frame1, E0(matchedPairs(:, 1), :), camStereoParams, pc);

    display(e);

    % write point cloud with marker (debugging)

    %pcDebug = pointCloud([pc.Location; E{i}], 'Color', [pc.Color; repmat([255, 0, 0], size(E{i}, 1), 1)]);

    %pcwrite(pcDebug, 'pcDebug.ply');

    % bring markers 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

    % bring point cloud into initial alignment

    pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);

    pc = pcread(pcFileName);

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

%     pcg = pctransform(pc, tform);

    pcg = pointCloud(pc.Location * initialAlign(i).Rotation' + initialAlign(i).Translation', 'Color', pc.Color);

    pcshow(pcg);

    xlabel('x');

    ylabel('y');

    zlabel('z');

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

    title('Initial Alignment');

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

% show found markers in optimized alignment

figure;

hold('on');

for i=1:nSubScans

    % fix Ri to be orthogonal

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

    %Ri = U*V';

    Ri = alignment(i).Rotation;

    Ti = alignment(i).Translation;

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

    % bring point cloud into optimized alignment

    pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);

    pc = pcread(pcFileName);

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

%     pcg = pctransform(pc, tform);

    pcg = pointCloud(pc.Location * alignment(i).Rotation' + alignment(i).Translation', 'Color', pc.Color);

    pcshow(pcg);

    xlabel('x');

    ylabel('y');

    zlabel('z');

    plot3(Ea(:,1), Ea(:,2), Ea(:,3), '.', 'MarkerSize', 15);

    title('Optimized Alignment');

end

% write to ALN file

for i=1:length(alignment)

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

end

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

end

function e = autoDetectMarkers(frame, P, pointCloud)

    % create mask based on morphology

    g = rgb2gray(frame);

    % g(g>254) = 0;

    % bw = imbinarize(g, 'adaptive', 'Sensitivity', 10^(-50));

    bw = imbinarize(g, 0.10);

    cc = bwconncomp(bw);

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

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

    e = cat(1, rp(idx).Centroid);

end

function e = manuallyDetectMarkers(frame, 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(caller, ~)

        p = get(gca, 'CurrentPoint'); 

        p = p(1, 1:2);

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

        if(not(isempty(el)))

            figure(caller);

            hold('on');

            plot(el(1), el(2), 'rx', 'MarkerSize', 15);

        end

    end

end

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

% Detect a marker in a single frame by rectifying to the image and

% performing image registration.

    % create mask based on morphology

    g = rgb2gray(frame);

    g(g>254) = 0;

    bw = imbinarize(g);

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

    e = zeros(size(initGuesses));

    conf = zeros(size(initGuesses, 1), 1);

    nMarkersFound = 0;

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

        if(sum(sum(labelMask)) < 10 || sum(sum(labelMask)) > 1000)

            continue;

        end

        % determine 3D points that are part of the marker

        % note: we should probably undistort labelMask

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

        for j=1:size(q,1)

            if(round(q(j,2)) > size(labelMask, 1) || round(q(j,1)) > size(labelMask, 2) || round(q(j,2)) < 1 || round(q(j,1)) < 1)

                continue;

            end

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

                pointMask(j) = true;

            end

        end

        if(sum(pointMask)) < 10

            continue;

        end

        % project 3D points onto local plane

        [~, sc, ~] = pca(Q(pointMask, :));

        Qlocal = sc(:, 1:2);

        % synthetic marker in high res. space

        m = zeros(151, 151);

        [x, y] = meshgrid(1:151, 1:151);

        m((x(:)-76).^2 + (y(:)-76).^2 <= 50^2) = 1.0;

        % relation between marker space (px) and true marker/local plane(mm)

        % true marker diameter is 1.75mm

        mScale = 101/1.4; %px/mm

        mShift = 76; %px

        % build homography from image to marker space

        H = fitgeotrans(q(pointMask, 1:2), mScale*Qlocal+mShift,  'projective');

        %Hdlt = Hest_DLT([mScale*Qlocal+mShift, ones(size(Qlocal, 1), 1)]', q(pointMask,:)');

        %H = projective2d(Hdlt');

        % bring image of marker into marker space

        imMarkerSpace = imwarp(frame, H, 'OutputView', imref2d(size(m)));

        imMarkerSpace = rgb2gray(im2double(imMarkerSpace));

        %figure; imshowpair(imMarkerSpace, m);

        % perform image registration

        % might be better off using subpixel image correlation

        [opt, met] = imregconfig('multimodal');

        T = imregtform(m, imMarkerSpace, 'translation', opt, met, 'DisplayOptimization', false);

        rege = imwarp(m, T, 'OutputView', imref2d(size(m)));

        %figure; imshowpair(imMarkerSpace, rege);

        % measure of correlation

        confI = sum(sum(imMarkerSpace .* rege))/sqrt(sum(sum(imMarkerSpace) * sum(sum(rege))));

        %confI = 1.0;

        if confI<0.4

            continue;

        end

        fprintf('Found marker with confidence: %f\n', confI);

        % transform marker space coordinates (76,76) to frame space

        el = T.transformPointsForward([76, 76]);

        el = H.transformPointsInverse(el);

        nMarkersFound = nMarkersFound+1;

        e(nMarkersFound,:) = el;

        conf(nMarkersFound) = confI;

    end

    e = e(1:nMarkersFound, :);

    conf = conf(1:nMarkersFound);

end

function [E, conf] = detectMarkersStereoSubpix(frame0, frame1, initGuesses, camStereoParams, pc)

% Detect a marker in stereo frame set by minimizing the difference to

% projected images of 3d marker

    normals = pcnormals(pc, 6);

    frame0 = rgb2gray(frame0);

    frame1 = rgb2gray(frame1);

    nMarkers = size(initGuesses, 1);

    E = zeros(nMarkers, 3);

    conf = zeros(nMarkers, 1);

    for i=1:nMarkers

        pI = initGuesses(i,:);

        e0 = camStereoParams.CameraParameters1.worldToImage(eye(3,3), zeros(3,1), pI);

        e1 = camStereoParams.CameraParameters2.worldToImage(camStereoParams.RotationOfCamera2, camStereoParams.TranslationOfCamera2, pI);

        % center of support region

        e0Center = round(e0);

        e1Center = round(e1);

        % find initial normal

        idx = pc.findNearestNeighbors(pI, 1);

        nI = double(normals(idx, :));

        margin = 25;

        [x,y] = meshgrid(e0Center(1)-margin:e0Center(1)+margin, e0Center(2)-margin:e0Center(2)+margin);

        e0ImCoords = [x(:), y(:)];

        [x,y] = meshgrid(e1Center(1)-margin:e1Center(1)+margin, e1Center(2)-margin:e1Center(2)+margin);

        e1ImCoords = [x(:), y(:)];

        nCoords = length(x(:));

        e0UndistImCoords = undistortPointsFast(e0ImCoords, camStereoParams.CameraParameters1);

        e0NormImCoords = camStereoParams.CameraParameters1.pointsToWorld(eye(3,3), [0, 0, 1], e0UndistImCoords);

        e0Hom = [e0NormImCoords, ones(nCoords, 1)];

        e1UndistImCoords = undistortPointsFast(e1ImCoords, camStereoParams.CameraParameters2);

        e1NormImCoords = camStereoParams.CameraParameters2.pointsToWorld(eye(3,3), [0, 0, 1], e1UndistImCoords);

        e1Hom = [e1NormImCoords, ones(nCoords, 1)];

        imVals0 = double(frame0(sub2ind(size(frame0), e0ImCoords(:,2), e0ImCoords(:,1))));

        imVals1 = double(frame1(sub2ind(size(frame1), e1ImCoords(:,2), e1ImCoords(:,1))));

        x0 = [pI nI(1:2)/nI(3) 70.0 70.0];

%         r = circleResiduals(x0);

%         figure; 

%         subplot(2,2,1);

%         imagesc(reshape(r(1:length(e0NormImCoords)), 2*margin+1, 2*margin+1), [-50 50]);

%         subplot(2,2,2);

%         imagesc(reshape(r(length(e0NormImCoords)+1:end), 2*margin+1, 2*margin+1), [-50 50]);

%         drawnow;

        %options = optimset('Algorithm', 'levenberg-marquardt', 'Display', 'final', 'OutputFcn', @out, 'MaxIter', 30, 'TolFun', 10^(-6), 'TolX', 0);

        options = optimoptions('lsqnonlin', 'Display', 'final');

        [x, conf(i), ~] = lsqnonlin(@circleResiduals, x0, [], [], options);

%         r = circleResiduals(x);

%         subplot(2,2,3);

%         imagesc(reshape(r(1:length(e0NormImCoords)), 2*margin+1, 2*margin+1), [-50 50]);

%         subplot(2,2,4);

%         imagesc(reshape(r(length(e0NormImCoords)+1:end), 2*margin+1, 2*margin+1), [-50 50]);

%         drawnow;

        E(i,:) = x(1:3);

    end

    function stop = out(x, optimValues, state)

%         r = optimValues.residual;

%         

%         figure; 

%         subplot(1,2,1);

%         imagesc(reshape(r(1:length(e0NormImCoords)), 2*margin+1, 2*margin+1), [-50 50]);

%         subplot(1,2,2);

%         imagesc(reshape(r(length(e0NormImCoords)+1:end), 2*margin+1, 2*margin+1), [-50 50]);

%         drawnow;

%         

%         display(x);

        stop = false;

    end

    function r = circleResiduals(x)

        p0 = x(1:3);

        p1 = x(1:3) * camStereoParams.RotationOfCamera2 + camStereoParams.TranslationOfCamera2;

        n0 = [x(4:5) 1.0];

        n0 = 1.0/norm(n0) * n0;

        n1 = n0 * camStereoParams.RotationOfCamera2;

        r = zeros(length(e0NormImCoords) + length(e1NormImCoords), 1);

        % norminal radius of markers

        markerRadius = 1.4/2.0; %mm

        % half-width of ramp

        w = 0.1; %mm

        % camera 0

        t = (p0*n0')./(e0Hom*n0');

        dVec = repmat(p0, nCoords, 1) - t.*e0Hom;

        d = sqrt(sum(dVec.*dVec, 2));

        % "saturated" ramp function for marker disc shape

        weights = max(min(1.0, -1.0/(2*w)*(d-markerRadius)+0.5), 0.0);

        r(1:nCoords) = x(6)*weights - imVals0;

        % camera 1

        t = (p1*n1')./(e1Hom*n1');

        dVec = repmat(p1, nCoords, 1) - t.*e1Hom;

        d = sqrt(sum(dVec.*dVec, 2));

        % "saturated" ramp function for marker disc shape

        weights = max(min(1.0, -1.0/(2*w)*(d-markerRadius)+0.5), 0.0);

        r(nCoords+1:end) = x(7)*weights - imVals1;

%         figure; 

%         subplot(1,2,1);

%         imagesc(reshape(r(1:length(e0NormImCoords)), 2*margin+1, 2*margin+1), [-50 50]);

%         subplot(1,2,2);

%         imagesc(reshape(r(length(e0NormImCoords)+1:end), 2*margin+1, 2*margin+1), [-50 50]);

%         drawnow;

    end

end

function E = projectOntoPointCloud(e, P, pointCloud)

% Project 2d point coordinates onto pointCloud to find corresponding 3d

% point coordinates.

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

        [sqDistsSorted, sortIdx] = sort(sqDists);

        % neighbors are points that project to within 10px

        neighbors = (sqDistsSorted < 10.0^2);

        distsSorted = sqrt(sqDistsSorted(neighbors));

        invDistsSorted = 1.0./distsSorted;

        sortIdx = sortIdx(neighbors);

        nNeighbors = sum(neighbors);

        if(nNeighbors >= 2)

            E(i, :) = 0;

            for j=1:nNeighbors

                E(i, :) = E(i, :) + invDistsSorted(j)/sum(invDistsSorted) * pointCloud(sortIdx(j), :);

            end

        end

    end    

end

function H = Hest_DLT(q1, q2)

    % Estimate the homography between a set of point correspondences using the 

    % direct linear transform algorithm.

    %

    % Input:

    %           q1: 3xN matrix of homogenous point coordinates from camera 1. 

    %           q2: 3xN matrix of corresponding points from camera 2.

    % Output:

    %           H: 3x3 matrix. The Fundamental Matrix estimate. 

    %

    % Note that N must be at least 4.

    % See derivation in Aanaes, Lecture Notes on Computer Vision, 2011

    % Normalize points

    [T1,invT1] = normalizationMat(q1);

    q1_tilde = T1*q1;

    T2 = normalizationMat(q2);

    q2_tilde = T2*q2;

    % DLT estimation

    N = size(q1_tilde,2);

    assert(size(q2_tilde,2)==N);

    B = zeros(3*N,9);

    for i=1:N

        q1i = q1_tilde(:,i);

        q2i = q2_tilde(:,i);

        q1_x = [0 -q1i(3) q1i(2); q1i(3) 0 -q1i(1); -q1i(2) q1i(1) 0];

        biT = kron(q2i', q1_x); 

        B(3*(i-1)+1:3*i, :) = biT;

    end

    [U,S,~] = svd(B');

    [~,idx] = min(diag(S));

    h = U(:,idx);

    H_tilde = reshape(h, 3, 3);

    % Unnormalize H

    H = invT1*H_tilde*T2;

    % Arbitrarily chose scale

    H = H * 1/H(3,3);

end

function [T,invT] = normalizationMat(q)

    % Gives a normalization matrix for homogeneous coordinates

    % such that T*q will have zero mean and unit variance.

    % See Aanaes, Computer Vision Lecture Notes 2.8.2

    %

    % q: (M+1)xN matrix of N MD points in homogenous coordinates

    %

    % Extended to also efficiently compute the inverse matrix

    % DTU, 2013, Jakob Wilm

    [M,N] = size(q);

    M = M-1;

    mu = mean(q(1:M,:),2);

    q_bar = q(1:M,:)-repmat(mu,1,N);

    s = mean(sqrt(diag(q_bar'*q_bar)))/sqrt(2);

    T = [eye(M)/s, -mu/s; zeros(1,M) 1];

    invT = [eye(M)*s, mu; zeros(1,M) 1];

end