WebSVN – seema-scanner – Diff – /matlab/alignSubScansMarkers.m/


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(2475:end, :) = 0;
    % 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
 
Generated by GNU Enscript 1.6.6.
 
 
 

Rev 243	Rev 246
1	`function MSE = alignSubScansMarkers(calibrationFileName, alnFileName)`	1	`function MSE = alignSubScansMarkers(calibrationFileName, alnFileName)`
2	`%ALIGNSUBSCANSMARKERS Determines an exact alignment of sub scans (scans`	2	`%ALIGNSUBSCANSMARKERS Determines an exact alignment of sub scans (scans`
3	`% from e.g. one revolution of the rotation stage).`	3	`% from e.g. one revolution of the rotation stage).`
4	`% The method searches for circular white markers of a specific diameter.`	4	`% The method searches for circular white markers of a specific diameter.`
5	`% White frames corresponding to each sub scan must be available.`	5	`% White frames corresponding to each sub scan must be available.`
6	`% A coarse alignment in the form of an aln-file must be provided.`	6	`% A coarse alignment in the form of an aln-file must be provided.`
7	`%`	7	`%`
8	`% 2017 Jakob Wilm, DTU`	8	`% 2017 Jakob Wilm, DTU`
9		9
10	`initialAlign = readMeshLabALN(alnFileName);`	10	`initialAlign = readMeshLabALN(alnFileName);`
11	`[alnFilePath, ~, ~] = fileparts(alnFileName);`	11	`[alnFilePath, ~, ~] = fileparts(alnFileName);`
12		12
13	`calibration = readOpenCVXML(calibrationFileName);`	13	`calibration = readOpenCVXML(calibrationFileName);`
14		14
15	`% correct for Matlab 1-indexing in principle point coordinates`	15	`% correct for Matlab 1-indexing in principle point coordinates`
16	`calibration.K0(1:2, 3) = calibration.K0(1:2, 3)+1;`	16	`calibration.K0(1:2, 3) = calibration.K0(1:2, 3)+1;`
17	`calibration.K1(1:2, 3) = calibration.K1(1:2, 3)+1;`	17	`calibration.K1(1:2, 3) = calibration.K1(1:2, 3)+1;`
18		18
19	`% full projection matrices in Matlab convention`	19	`% full projection matrices in Matlab convention`
20	`P0 = transpose(calibration.K0*[eye(3) zeros(3,1)]);`	20	`P0 = transpose(calibration.K0*[eye(3) zeros(3,1)]);`
21	`P1 = transpose(calibration.K1*[calibration.R1 calibration.T1']);`	21	`P1 = transpose(calibration.K1*[calibration.R1 calibration.T1']);`
22		22
23	`% matlab cam params for undistortion`	23	`% matlab cam params for undistortion`
24	`camParams0 = cameraParameters('IntrinsicMatrix', calibration.K0', 'RadialDistortion', calibration.k0([1 2 5]), 'TangentialDistortion', calibration.k0([3 4]));`	24	`camParams0 = cameraParameters('IntrinsicMatrix', calibration.K0', 'RadialDistortion', calibration.k0([1 2 5]), 'TangentialDistortion', calibration.k0([3 4]));`
25	`camParams1 = cameraParameters('IntrinsicMatrix', calibration.K1', 'RadialDistortion', calibration.k1([1 2 5]), 'TangentialDistortion', calibration.k1([3 4]));`	25	`camParams1 = cameraParameters('IntrinsicMatrix', calibration.K1', 'RadialDistortion', calibration.k1([1 2 5]), 'TangentialDistortion', calibration.k1([3 4]));`
26		26
27	`% matlab struct for triangulation`	27	`% matlab struct for triangulation`
28	`camStereoParams = stereoParameters(camParams0, camParams1, calibration.R1', calibration.T1');`	28	`camStereoParams = stereoParameters(camParams0, camParams1, calibration.R1', calibration.T1');`
29		29
30	`nSubScans = length(initialAlign);`	30	`nSubScans = length(initialAlign);`
31	`%nSubScans = 5;`	31	`%nSubScans = 5;`
32		32
33	`% 3D coordinates of markers in local camera frame`	33	`% 3D coordinates of markers in local camera frame`
34	`E = cell(nSubScans, 1);`	34	`E = cell(nSubScans, 1);`
35		35
36	`% 3D coordinates of markers in global initial alignment`	36	`% 3D coordinates of markers in global initial alignment`
37	`Eg = cell(size(E));`	37	`Eg = cell(size(E));`
38		38
39	`% find 3D markers coordinates`	39	`% find 3D markers coordinates`
40	`for i=1:nSubScans`	40	`for i=1:nSubScans`
41	`% load point cloud`	41	`% load point cloud`
42	`pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);`	42	`pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);`
43	`pcFilePath = fileparts(pcFileName);`	43	`pcFilePath = fileparts(pcFileName);`
44	`pc = pcread(pcFileName);`	44	`pc = pcread(pcFileName);`
45	`Q = pc.Location;`	45	`Q = pc.Location;`
46	`idString = strsplit(initialAlign(i).FileName, {'.ply', '_'});`	46	`idString = strsplit(initialAlign(i).FileName, {'.ply', '_'});`
47	`idString = idString{end-1};`	47	`idString = idString{end-1};`
48		48
49	`% load white frames`	49	`% load white frames`
50	`frame0 = imread(fullfile(pcFilePath, ['sequence_' idString], 'frames0_0.png'));`	50	`frame0 = imread(fullfile(pcFilePath, ['sequence_' idString], 'frames0_0.png'));`
51	`frame1 = imread(fullfile(pcFilePath, ['sequence_' idString], 'frames1_0.png'));`	51	`frame1 = imread(fullfile(pcFilePath, ['sequence_' idString], 'frames1_0.png'));`
52		52
53	`e0Coords = autoDetectMarkers(frame0);`	53	`e0Coords = autoDetectMarkers(frame0);`
54	`e1Coords = autoDetectMarkers(frame1);`	54	`e1Coords = autoDetectMarkers(frame1);`
55		55
56	`%e0Coords = manuallyDetectMarkers(frame0);`	56	`%e0Coords = manuallyDetectMarkers(frame0);`
57	`%e1Coords = manuallyDetectMarkers(frame1);`	57	`%e1Coords = manuallyDetectMarkers(frame1);`
58		58
59	`%[e0Coords, conf0] = detectMarkersSubpix(frame0, e0Coords, P0, Q);`	59	`%[e0Coords, conf0] = detectMarkersSubpix(frame0, e0Coords, P0, Q);`
60	`%[e1Coords, conf1] = detectMarkersSubpix(frame1, e1Coords, P1, Q);`	60	`%[e1Coords, conf1] = detectMarkersSubpix(frame1, e1Coords, P1, Q);`
61		61
62	`if(length(e0Coords) < 1 \|\| length(e1Coords) < 1)`	62	`if(length(e0Coords) < 1 \|\| length(e1Coords) < 1)`
63	`continue;`	63	`continue;`
64	`end`	64	`end`
65		65
66	`% figure;`	66	`% figure;`
67	`% subplot(1,2,1);`	67	`% subplot(1,2,1);`
68	`% imshow(frame0);`	68	`% imshow(frame0);`
69	`% hold('on');`	69	`% hold('on');`
70	`% plot(e0Coords(:,1), e0Coords(:,2), 'rx', 'MarkerSize', 15);`	70	`% plot(e0Coords(:,1), e0Coords(:,2), 'rx', 'MarkerSize', 15);`
71	`% subplot(1,2,2);`	71	`% subplot(1,2,2);`
72	`% imshow(frame1);`	72	`% imshow(frame1);`
73	`% hold('on');`	73	`% hold('on');`
74	`% plot(e1Coords(:,1), e1Coords(:,2), 'rx', 'MarkerSize', 15);`	74	`% plot(e1Coords(:,1), e1Coords(:,2), 'rx', 'MarkerSize', 15);`
75	`% drawnow;`	75	`% drawnow;`
76		76
77	`e0CoordsUndistort = undistortPointsFast(e0Coords, camParams0);`	77	`e0CoordsUndistort = undistortPointsFast(e0Coords, camParams0);`
78	`e1CoordsUndistort = undistortPointsFast(e1Coords, camParams1);`	78	`e1CoordsUndistort = undistortPointsFast(e1Coords, camParams1);`
79		79
80	`% match ellipse candidates between cameras based on projection`	80	`% match ellipse candidates between cameras based on projection`
81	`E0 = projectOntoPointCloud(e0CoordsUndistort, P0, Q);`	81	`E0 = projectOntoPointCloud(e0CoordsUndistort, P0, Q);`
82	`E1 = projectOntoPointCloud(e1CoordsUndistort, P1, Q);`	82	`E1 = projectOntoPointCloud(e1CoordsUndistort, P1, Q);`
83		83
84	`matchedPairs = nan(size(E0, 1), 2);`	84	`matchedPairs = nan(size(E0, 1), 2);`
85	`nMatchedPairs = 0;`	85	`nMatchedPairs = 0;`
86	`for j=1:size(E0, 1)`	86	`for j=1:size(E0, 1)`
87		87
88	`% should use pdist2 instead..`	88	`% should use pdist2 instead..`
89	`sqDists = sum((E1 - repmat(E0(j,:), size(E1, 1), 1)).^2, 2);`	89	`sqDists = sum((E1 - repmat(E0(j,:), size(E1, 1), 1)).^2, 2);`
90		90
91	`[minSqDist, minSqDistIdx] = min(sqDists);`	91	`[minSqDist, minSqDistIdx] = min(sqDists);`
92		92
93	`if(minSqDist < 2^2)`	93	`if(minSqDist < 2^2)`
94	`nMatchedPairs = nMatchedPairs + 1;`	94	`nMatchedPairs = nMatchedPairs + 1;`
95	`matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];`	95	`matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];`
96	`end`	96	`end`
97	`end`	97	`end`
98	`matchedPairs = matchedPairs(1:nMatchedPairs, :);`	98	`matchedPairs = matchedPairs(1:nMatchedPairs, :);`
99		99
100	`figure;`	100	`figure;`
101	`subplot(1,2,1);`	101	`subplot(1,2,1);`
102	`imshow(frame0);`	102	`imshow(frame0);`
103	`hold('on');`	103	`hold('on');`
104	`plot(e0Coords(matchedPairs(:, 1),1), e0Coords(matchedPairs(:, 1),2), 'rx', 'MarkerSize', 15);`	104	`plot(e0Coords(matchedPairs(:, 1),1), e0Coords(matchedPairs(:, 1),2), 'rx', 'MarkerSize', 15);`
105	`subplot(1,2,2);`	105	`subplot(1,2,2);`
106	`imshow(frame1);`	106	`imshow(frame1);`
107	`hold('on');`	107	`hold('on');`
108	`plot(e1Coords(matchedPairs(:, 2),1), e1Coords(matchedPairs(:, 2),2), 'rx', 'MarkerSize', 15);`	108	`plot(e1Coords(matchedPairs(:, 2),1), e1Coords(matchedPairs(:, 2),2), 'rx', 'MarkerSize', 15);`
109	`drawnow;`	109	`drawnow;`
110		110
111	`% % triangulate marker centers (lens correction has been performed)`	111	`% % triangulate marker centers (lens correction has been performed)`
112	`% [E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);`	112	`% [E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);`
113	`% E{i} = E{i}(e<3.0, :);`	113	`% E{i} = E{i}(e<3.0, :);`
114	`% display(e);`	114	`% display(e);`
115		115
116	`[E{i}, e] = detectMarkersStereoSubpix(frame0, frame1, E0(matchedPairs(:, 1), :), camStereoParams, pc);`	116	`[E{i}, e] = detectMarkersStereoSubpix(frame0, frame1, E0(matchedPairs(:, 1), :), camStereoParams, pc);`
117	`display(e);`	117	`display(e);`
118		118
119	`% write point cloud with marker (debugging)`	119	`% write point cloud with marker (debugging)`
120	`%pcDebug = pointCloud([pc.Location; E{i}], 'Color', [pc.Color; repmat([255, 0, 0], size(E{i}, 1), 1)]);`	120	`%pcDebug = pointCloud([pc.Location; E{i}], 'Color', [pc.Color; repmat([255, 0, 0], size(E{i}, 1), 1)]);`
121	`%pcwrite(pcDebug, 'pcDebug.ply');`	121	`%pcwrite(pcDebug, 'pcDebug.ply');`
122		122
123	`% bring markers into initial alignment`	123	`% bring markers into initial alignment`
124	`[U,~,V] = svd(initialAlign(i).Rotation);`	124	`[U,~,V] = svd(initialAlign(i).Rotation);`
125	`Ri = U*V';`	125	`Ri = U*V';`
126	`Ti = initialAlign(i).Translation;`	126	`Ti = initialAlign(i).Translation;`
127		127
128	`Eg{i} = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);`	128	`Eg{i} = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);`
129	`end`	129	`end`
130		130
131	`% show found markers in initial alignment`	131	`% show found markers in initial alignment`
132	`figure;`	132	`figure;`
133	`hold('on');`	133	`hold('on');`
134	`for i=1:nSubScans`	134	`for i=1:nSubScans`
135		135
136	`% bring point cloud into initial alignment`	136	`% bring point cloud into initial alignment`
137	`pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);`	137	`pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);`
138	`pc = pcread(pcFileName);`	138	`pc = pcread(pcFileName);`
139	`% tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);`	139	`% tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);`
140	`% pcg = pctransform(pc, tform);`	140	`% pcg = pctransform(pc, tform);`
141	`pcg = pointCloud(pc.Location * initialAlign(i).Rotation' + initialAlign(i).Translation', 'Color', pc.Color);`	141	`pcg = pointCloud(pc.Location * initialAlign(i).Rotation' + initialAlign(i).Translation', 'Color', pc.Color);`
142		142
143	`pcshow(pcg);`	143	`pcshow(pcg);`
144	`xlabel('x');`	144	`xlabel('x');`
145	`ylabel('y');`	145	`ylabel('y');`
146	`zlabel('z');`	146	`zlabel('z');`
147		147
148	`plot3(Eg{i}(:,1), Eg{i}(:,2), Eg{i}(:,3), '.', 'MarkerSize', 15);`	148	`plot3(Eg{i}(:,1), Eg{i}(:,2), Eg{i}(:,3), '.', 'MarkerSize', 15);`
149	`title('Initial Alignment');`	149	`title('Initial Alignment');`
150	`end`	150	`end`
151		151
152	`% match markers between poses using initial alignment`	152	`% match markers between poses using initial alignment`
153	`Pg = {};`	153	`Pg = {};`
154	`P = {};`	154	`P = {};`
155	`for i=1:nSubScans`	155	`for i=1:nSubScans`
156	`for j=1:size(Eg{i}, 1)`	156	`for j=1:size(Eg{i}, 1)`
157	`pg = Eg{i}(j,:);`	157	`pg = Eg{i}(j,:);`
158	`p = E{i}(j,:);`	158	`p = E{i}(j,:);`
159	`matched = false;`	159	`matched = false;`
160	`for k=1:size(Pg, 2)`	160	`for k=1:size(Pg, 2)`
161	`clusterCenter = mean(cat(1, Pg{:,k}), 1);`	161	`clusterCenter = mean(cat(1, Pg{:,k}), 1);`
162	`if(sum((pg - clusterCenter).^2) < 3^2)`	162	`if(sum((pg - clusterCenter).^2) < 3^2)`
163	`% store in global frame`	163	`% store in global frame`
164	`Pg{i,k} = pg;`	164	`Pg{i,k} = pg;`
165	`% store in local frame`	165	`% store in local frame`
166	`P{i,k} = p;`	166	`P{i,k} = p;`
167	`matched = true;`	167	`matched = true;`
168	`break;`	168	`break;`
169	`end`	169	`end`
170	`end`	170	`end`
171	`% create new cluster`	171	`% create new cluster`
172	`if(not(matched))`	172	`if(not(matched))`
173	`Pg{i,end+1} = pg;`	173	`Pg{i,end+1} = pg;`
174	`P{i,end+1} = p;`	174	`P{i,end+1} = p;`
175	`end`	175	`end`
176	`end`	176	`end`
177	`end`	177	`end`
178		178
179	`% run optimization`	179	`% run optimization`
180	`alignment = groupwiseOrthogonalProcrustes(P, initialAlign);`	180	`alignment = groupwiseOrthogonalProcrustes(P, initialAlign);`
181		181
182	`% show found markers in optimized alignment`	182	`% show found markers in optimized alignment`
183	`figure;`	183	`figure;`
184	`hold('on');`	184	`hold('on');`
185	`for i=1:nSubScans`	185	`for i=1:nSubScans`
186	`% fix Ri to be orthogonal`	186	`% fix Ri to be orthogonal`
187	`%[U,~,V] = svd(alignment(i).Rotation);`	187	`%[U,~,V] = svd(alignment(i).Rotation);`
188	`%Ri = U*V';`	188	`%Ri = U*V';`
189	`Ri = alignment(i).Rotation;`	189	`Ri = alignment(i).Rotation;`
190	`Ti = alignment(i).Translation;`	190	`Ti = alignment(i).Translation;`
191		191
192	`Ea = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);`	192	`Ea = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);`
193		193
194	`% bring point cloud into optimized alignment`	194	`% bring point cloud into optimized alignment`
195	`pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);`	195	`pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);`
196	`pc = pcread(pcFileName);`	196	`pc = pcread(pcFileName);`
197	`% tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);`	197	`% tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);`
198	`% pcg = pctransform(pc, tform);`	198	`% pcg = pctransform(pc, tform);`
199	`pcg = pointCloud(pc.Location * alignment(i).Rotation' + alignment(i).Translation', 'Color', pc.Color);`	199	`pcg = pointCloud(pc.Location * alignment(i).Rotation' + alignment(i).Translation', 'Color', pc.Color);`
200		200
201	`pcshow(pcg);`	201	`pcshow(pcg);`
202	`xlabel('x');`	202	`xlabel('x');`
203	`ylabel('y');`	203	`ylabel('y');`
204	`zlabel('z');`	204	`zlabel('z');`
205		205
206	`plot3(Ea(:,1), Ea(:,2), Ea(:,3), '.', 'MarkerSize', 15);`	206	`plot3(Ea(:,1), Ea(:,2), Ea(:,3), '.', 'MarkerSize', 15);`
207	`title('Optimized Alignment');`	207	`title('Optimized Alignment');`
208	`end`	208	`end`
209		209
210	`% write to ALN file`	210	`% write to ALN file`
211	`for i=1:length(alignment)`	211	`for i=1:length(alignment)`
212	`alignment(i).FileName = initialAlign(i).FileName;`	212	`alignment(i).FileName = initialAlign(i).FileName;`
213	`end`	213	`end`
214		214
215	`writeMeshLabALN(alignment, strrep(alnFileName, '.aln', 'Optimized.aln'));`	215	`writeMeshLabALN(alignment, strrep(alnFileName, '.aln', 'Optimized.aln'));`
216		216
217	`end`	217	`end`
218		218
219	`function e = autoDetectMarkers(frame, P, pointCloud)`	219	`function e = autoDetectMarkers(frame, P, pointCloud)`
220		220
221	`% create mask based on morphology`	221	`% create mask based on morphology`
222	`g = rgb2gray(frame);`	222	`g = rgb2gray(frame);`
-		223	`g(2475:end, :) = 0;`
223	`% g(g>254) = 0;`	224	`% g(g>254) = 0;`
224	`% bw = imbinarize(g, 'adaptive', 'Sensitivity', 10^(-50));`	225	`% bw = imbinarize(g, 'adaptive', 'Sensitivity', 10^(-50));`
225	`bw = imbinarize(g, 0.10);`	226	`bw = imbinarize(g, 0.10);`
226	`cc = bwconncomp(bw);`	227	`cc = bwconncomp(bw);`
227	`rp = regionprops(cc, 'Area', 'Solidity', 'Eccentricity', 'Centroid');`	228	`rp = regionprops(cc, 'Area', 'Solidity', 'Eccentricity', 'Centroid');`
228	`idx = ([rp.Area] > 100 & [rp.Area] < 1000 & [rp.Solidity] > 0.9);`	229	`idx = ([rp.Area] > 100 & [rp.Area] < 1000 & [rp.Solidity] > 0.9);`
229		230
230	`e = cat(1, rp(idx).Centroid);`	231	`e = cat(1, rp(idx).Centroid);`
231		232
232	`end`	233	`end`
233		234
234	`function e = manuallyDetectMarkers(frame, P, pointCloud)`	235	`function e = manuallyDetectMarkers(frame, P, pointCloud)`
235		236
236	`e = [];`	237	`e = [];`
237	`%edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);`	238	`%edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);`
238		239
239	`figure;`	240	`figure;`
240	`hold('on');`	241	`hold('on');`
241	`imshow(frame);`	242	`imshow(frame);`
242	`title('Close figure to end.');`	243	`title('Close figure to end.');`
243	`set(gcf, 'pointer', 'crosshair');`	244	`set(gcf, 'pointer', 'crosshair');`
244	`set(gcf, 'WindowButtonDownFcn', @clickCallback);`	245	`set(gcf, 'WindowButtonDownFcn', @clickCallback);`
245		246
246	`uiwait;`	247	`uiwait;`
247		248
248	`function clickCallback(caller, ~)`	249	`function clickCallback(caller, ~)`
249		250
250	`p = get(gca, 'CurrentPoint');`	251	`p = get(gca, 'CurrentPoint');`
251	`p = p(1, 1:2);`	252	`p = p(1, 1:2);`
252		253
253	`e = [e; p(:, 1:2)];`	254	`e = [e; p(:, 1:2)];`
254		255
255	`if(not(isempty(el)))`	256	`if(not(isempty(el)))`
256	`figure(caller);`	257	`figure(caller);`
257	`hold('on');`	258	`hold('on');`
258	`plot(el(1), el(2), 'rx', 'MarkerSize', 15);`	259	`plot(el(1), el(2), 'rx', 'MarkerSize', 15);`
259	`end`	260	`end`
260	`end`	261	`end`
261		262
262	`end`	263	`end`
263		264
264	`function [e, conf] = detectMarkersSubpix(frame, initGuesses, P, Q)`	265	`function [e, conf] = detectMarkersSubpix(frame, initGuesses, P, Q)`
265	`% Detect a marker in a single frame by rectifying to the image and`	266	`% Detect a marker in a single frame by rectifying to the image and`
266	`% performing image registration.`	267	`% performing image registration.`
267		268
268	`% create mask based on morphology`	269	`% create mask based on morphology`
269	`g = rgb2gray(frame);`	270	`g = rgb2gray(frame);`
270	`g(g>254) = 0;`	271	`g(g>254) = 0;`
271	`bw = imbinarize(g);`	272	`bw = imbinarize(g);`
272	`cc = bwconncomp(bw);`	273	`cc = bwconncomp(bw);`
273	`labels = labelmatrix(cc);`	274	`labels = labelmatrix(cc);`
274		275
275	`% project point cloud into image`	276	`% project point cloud into image`
276	`q = [Q ones(size(Q,1),1)]*P;`	277	`q = [Q ones(size(Q,1),1)]*P;`
277	`q = q./[q(:,3) q(:,3) q(:,3)];`	278	`q = q./[q(:,3) q(:,3) q(:,3)];`
278		279
279	`e = zeros(size(initGuesses));`	280	`e = zeros(size(initGuesses));`
280	`conf = zeros(size(initGuesses, 1), 1);`	281	`conf = zeros(size(initGuesses, 1), 1);`
281		282
282	`nMarkersFound = 0;`	283	`nMarkersFound = 0;`
283		284
284	`for i=1:size(initGuesses, 1)`	285	`for i=1:size(initGuesses, 1)`
285		286
286	`labelId = labels(round(initGuesses(i,2)), round(initGuesses(i,1)));`	287	`labelId = labels(round(initGuesses(i,2)), round(initGuesses(i,1)));`
287	`labelMask = (labels == labelId);`	288	`labelMask = (labels == labelId);`
288	`labelMask = imdilate(labelMask, strel('disk', 3, 0));`	289	`labelMask = imdilate(labelMask, strel('disk', 3, 0));`
289		290
290	`if(sum(sum(labelMask)) < 10 \|\| sum(sum(labelMask)) > 1000)`	291	`if(sum(sum(labelMask)) < 10 \|\| sum(sum(labelMask)) > 1000)`
291	`continue;`	292	`continue;`
292	`end`	293	`end`
293		294
294	`% determine 3D points that are part of the marker`	295	`% determine 3D points that are part of the marker`
295	`% note: we should probably undistort labelMask`	296	`% note: we should probably undistort labelMask`
296	`pointMask = false(size(q, 1), 1);`	297	`pointMask = false(size(q, 1), 1);`
297	`for j=1:size(q,1)`	298	`for j=1:size(q,1)`
298	`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)`	299	`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)`
299	`continue;`	300	`continue;`
300	`end`	301	`end`
301		302
302	`if(labelMask(round(q(j,2)), round(q(j,1))))`	303	`if(labelMask(round(q(j,2)), round(q(j,1))))`
303	`pointMask(j) = true;`	304	`pointMask(j) = true;`
304	`end`	305	`end`
305	`end`	306	`end`
306		307
307	`if(sum(pointMask)) < 10`	308	`if(sum(pointMask)) < 10`
308	`continue;`	309	`continue;`
309	`end`	310	`end`
310		311
311	`% project 3D points onto local plane`	312	`% project 3D points onto local plane`
312	`[~, sc, ~] = pca(Q(pointMask, :));`	313	`[~, sc, ~] = pca(Q(pointMask, :));`
313	`Qlocal = sc(:, 1:2);`	314	`Qlocal = sc(:, 1:2);`
314		315
315	`% synthetic marker in high res. space`	316	`% synthetic marker in high res. space`
316	`m = zeros(151, 151);`	317	`m = zeros(151, 151);`
317	`[x, y] = meshgrid(1:151, 1:151);`	318	`[x, y] = meshgrid(1:151, 1:151);`
318	`m((x(:)-76).^2 + (y(:)-76).^2 <= 50^2) = 1.0;`	319	`m((x(:)-76).^2 + (y(:)-76).^2 <= 50^2) = 1.0;`
319		320
320	`% relation between marker space (px) and true marker/local plane(mm)`	321	`% relation between marker space (px) and true marker/local plane(mm)`
321	`% true marker diameter is 1.75mm`	322	`% true marker diameter is 1.75mm`
322	`mScale = 101/1.4; %px/mm`	323	`mScale = 101/1.4; %px/mm`
323	`mShift = 76; %px`	324	`mShift = 76; %px`
324		325
325	`% build homography from image to marker space`	326	`% build homography from image to marker space`
326	`H = fitgeotrans(q(pointMask, 1:2), mScale*Qlocal+mShift, 'projective');`	327	`H = fitgeotrans(q(pointMask, 1:2), mScale*Qlocal+mShift, 'projective');`
327	`%Hdlt = Hest_DLT([mScale*Qlocal+mShift, ones(size(Qlocal, 1), 1)]', q(pointMask,:)');`	328	`%Hdlt = Hest_DLT([mScale*Qlocal+mShift, ones(size(Qlocal, 1), 1)]', q(pointMask,:)');`
328	`%H = projective2d(Hdlt');`	329	`%H = projective2d(Hdlt');`
329		330
330	`% bring image of marker into marker space`	331	`% bring image of marker into marker space`
331	`imMarkerSpace = imwarp(frame, H, 'OutputView', imref2d(size(m)));`	332	`imMarkerSpace = imwarp(frame, H, 'OutputView', imref2d(size(m)));`
332	`imMarkerSpace = rgb2gray(im2double(imMarkerSpace));`	333	`imMarkerSpace = rgb2gray(im2double(imMarkerSpace));`
333		334
334	`%figure; imshowpair(imMarkerSpace, m);`	335	`%figure; imshowpair(imMarkerSpace, m);`
335		336
336	`% perform image registration`	337	`% perform image registration`
337	`% might be better off using subpixel image correlation`	338	`% might be better off using subpixel image correlation`
338	`[opt, met] = imregconfig('multimodal');`	339	`[opt, met] = imregconfig('multimodal');`
339	`T = imregtform(m, imMarkerSpace, 'translation', opt, met, 'DisplayOptimization', false);`	340	`T = imregtform(m, imMarkerSpace, 'translation', opt, met, 'DisplayOptimization', false);`
340		341
341	`rege = imwarp(m, T, 'OutputView', imref2d(size(m)));`	342	`rege = imwarp(m, T, 'OutputView', imref2d(size(m)));`
342	`%figure; imshowpair(imMarkerSpace, rege);`	343	`%figure; imshowpair(imMarkerSpace, rege);`
343		344
344	`% measure of correlation`	345	`% measure of correlation`
345	`confI = sum(sum(imMarkerSpace .* rege))/sqrt(sum(sum(imMarkerSpace) * sum(sum(rege))));`	346	`confI = sum(sum(imMarkerSpace .* rege))/sqrt(sum(sum(imMarkerSpace) * sum(sum(rege))));`
346	`%confI = 1.0;`	347	`%confI = 1.0;`
347		348
348	`if confI<0.4`	349	`if confI<0.4`
349	`continue;`	350	`continue;`
350	`end`	351	`end`
351		352
352	`fprintf('Found marker with confidence: %f\n', confI);`	353	`fprintf('Found marker with confidence: %f\n', confI);`
353		354
354	`% transform marker space coordinates (76,76) to frame space`	355	`% transform marker space coordinates (76,76) to frame space`
355	`el = T.transformPointsForward([76, 76]);`	356	`el = T.transformPointsForward([76, 76]);`
356	`el = H.transformPointsInverse(el);`	357	`el = H.transformPointsInverse(el);`
357		358
358	`nMarkersFound = nMarkersFound+1;`	359	`nMarkersFound = nMarkersFound+1;`
359	`e(nMarkersFound,:) = el;`	360	`e(nMarkersFound,:) = el;`
360	`conf(nMarkersFound) = confI;`	361	`conf(nMarkersFound) = confI;`
361	`end`	362	`end`
362		363
363	`e = e(1:nMarkersFound, :);`	364	`e = e(1:nMarkersFound, :);`
364	`conf = conf(1:nMarkersFound);`	365	`conf = conf(1:nMarkersFound);`
365	`end`	366	`end`
366		367
367	`function [E, conf] = detectMarkersStereoSubpix(frame0, frame1, initGuesses, camStereoParams, pc)`	368	`function [E, conf] = detectMarkersStereoSubpix(frame0, frame1, initGuesses, camStereoParams, pc)`
368	`% Detect a marker in stereo frame set by minimizing the difference to`	369	`% Detect a marker in stereo frame set by minimizing the difference to`
369	`% projected images of 3d marker`	370	`% projected images of 3d marker`
370		371
371	`normals = pcnormals(pc, 6);`	372	`normals = pcnormals(pc, 6);`
372		373
373	`frame0 = rgb2gray(frame0);`	374	`frame0 = rgb2gray(frame0);`
374	`frame1 = rgb2gray(frame1);`	375	`frame1 = rgb2gray(frame1);`
375		376
376	`nMarkers = size(initGuesses, 1);`	377	`nMarkers = size(initGuesses, 1);`
377		378
378	`E = zeros(nMarkers, 3);`	379	`E = zeros(nMarkers, 3);`
379	`conf = zeros(nMarkers, 1);`	380	`conf = zeros(nMarkers, 1);`
380		381
381	`for i=1:nMarkers`	382	`for i=1:nMarkers`
382		383
383	`pI = initGuesses(i,:);`	384	`pI = initGuesses(i,:);`
384		385
385	`e0 = camStereoParams.CameraParameters1.worldToImage(eye(3,3), zeros(3,1), pI);`	386	`e0 = camStereoParams.CameraParameters1.worldToImage(eye(3,3), zeros(3,1), pI);`
386	`e1 = camStereoParams.CameraParameters2.worldToImage(camStereoParams.RotationOfCamera2, camStereoParams.TranslationOfCamera2, pI);`	387	`e1 = camStereoParams.CameraParameters2.worldToImage(camStereoParams.RotationOfCamera2, camStereoParams.TranslationOfCamera2, pI);`
387		388
388	`% center of support region`	389	`% center of support region`
389	`e0Center = round(e0);`	390	`e0Center = round(e0);`
390	`e1Center = round(e1);`	391	`e1Center = round(e1);`
391		392
392	`% find initial normal`	393	`% find initial normal`
393	`idx = pc.findNearestNeighbors(pI, 1);`	394	`idx = pc.findNearestNeighbors(pI, 1);`
394	`nI = double(normals(idx, :));`	395	`nI = double(normals(idx, :));`
395		396
396	`margin = 25;`	397	`margin = 25;`
397		398
398	`[x,y] = meshgrid(e0Center(1)-margin:e0Center(1)+margin, e0Center(2)-margin:e0Center(2)+margin);`	399	`[x,y] = meshgrid(e0Center(1)-margin:e0Center(1)+margin, e0Center(2)-margin:e0Center(2)+margin);`
399	`e0ImCoords = [x(:), y(:)];`	400	`e0ImCoords = [x(:), y(:)];`
400		401
401	`[x,y] = meshgrid(e1Center(1)-margin:e1Center(1)+margin, e1Center(2)-margin:e1Center(2)+margin);`	402	`[x,y] = meshgrid(e1Center(1)-margin:e1Center(1)+margin, e1Center(2)-margin:e1Center(2)+margin);`
402	`e1ImCoords = [x(:), y(:)];`	403	`e1ImCoords = [x(:), y(:)];`
403		404
404	`nCoords = length(x(:));`	405	`nCoords = length(x(:));`
405		406
406	`e0UndistImCoords = undistortPointsFast(e0ImCoords, camStereoParams.CameraParameters1);`	407	`e0UndistImCoords = undistortPointsFast(e0ImCoords, camStereoParams.CameraParameters1);`
407	`e0NormImCoords = camStereoParams.CameraParameters1.pointsToWorld(eye(3,3), [0, 0, 1], e0UndistImCoords);`	408	`e0NormImCoords = camStereoParams.CameraParameters1.pointsToWorld(eye(3,3), [0, 0, 1], e0UndistImCoords);`
408	`e0Hom = [e0NormImCoords, ones(nCoords, 1)];`	409	`e0Hom = [e0NormImCoords, ones(nCoords, 1)];`
409	`e1UndistImCoords = undistortPointsFast(e1ImCoords, camStereoParams.CameraParameters2);`	410	`e1UndistImCoords = undistortPointsFast(e1ImCoords, camStereoParams.CameraParameters2);`
410	`e1NormImCoords = camStereoParams.CameraParameters2.pointsToWorld(eye(3,3), [0, 0, 1], e1UndistImCoords);`	411	`e1NormImCoords = camStereoParams.CameraParameters2.pointsToWorld(eye(3,3), [0, 0, 1], e1UndistImCoords);`
411	`e1Hom = [e1NormImCoords, ones(nCoords, 1)];`	412	`e1Hom = [e1NormImCoords, ones(nCoords, 1)];`
412		413
413	`imVals0 = double(frame0(sub2ind(size(frame0), e0ImCoords(:,2), e0ImCoords(:,1))));`	414	`imVals0 = double(frame0(sub2ind(size(frame0), e0ImCoords(:,2), e0ImCoords(:,1))));`
414	`imVals1 = double(frame1(sub2ind(size(frame1), e1ImCoords(:,2), e1ImCoords(:,1))));`	415	`imVals1 = double(frame1(sub2ind(size(frame1), e1ImCoords(:,2), e1ImCoords(:,1))));`
415		416
416	`x0 = [pI nI(1:2)/nI(3) 70.0 70.0];`	417	`x0 = [pI nI(1:2)/nI(3) 70.0 70.0];`
417		418
418	`% r = circleResiduals(x0);`	419	`% r = circleResiduals(x0);`
419	`% figure;`	420	`% figure;`
420	`% subplot(2,2,1);`	421	`% subplot(2,2,1);`
421	`% imagesc(reshape(r(1:length(e0NormImCoords)), 2margin+1, 2margin+1), [-50 50]);`	422	`% imagesc(reshape(r(1:length(e0NormImCoords)), 2margin+1, 2margin+1), [-50 50]);`
422	`% subplot(2,2,2);`	423	`% subplot(2,2,2);`
423	`% imagesc(reshape(r(length(e0NormImCoords)+1:end), 2margin+1, 2margin+1), [-50 50]);`	424	`% imagesc(reshape(r(length(e0NormImCoords)+1:end), 2margin+1, 2margin+1), [-50 50]);`
424	`% drawnow;`	425	`% drawnow;`
425		426
426	`%options = optimset('Algorithm', 'levenberg-marquardt', 'Display', 'final', 'OutputFcn', @out, 'MaxIter', 30, 'TolFun', 10^(-6), 'TolX', 0);`	427	`%options = optimset('Algorithm', 'levenberg-marquardt', 'Display', 'final', 'OutputFcn', @out, 'MaxIter', 30, 'TolFun', 10^(-6), 'TolX', 0);`
427	`options = optimoptions('lsqnonlin', 'Display', 'final');`	428	`options = optimoptions('lsqnonlin', 'Display', 'final');`
428		429
429	`[x, conf(i), ~] = lsqnonlin(@circleResiduals, x0, [], [], options);`	430	`[x, conf(i), ~] = lsqnonlin(@circleResiduals, x0, [], [], options);`
430		431
431	`% r = circleResiduals(x);`	432	`% r = circleResiduals(x);`
432	`% subplot(2,2,3);`	433	`% subplot(2,2,3);`
433	`% imagesc(reshape(r(1:length(e0NormImCoords)), 2margin+1, 2margin+1), [-50 50]);`	434	`% imagesc(reshape(r(1:length(e0NormImCoords)), 2margin+1, 2margin+1), [-50 50]);`
434	`% subplot(2,2,4);`	435	`% subplot(2,2,4);`
435	`% imagesc(reshape(r(length(e0NormImCoords)+1:end), 2margin+1, 2margin+1), [-50 50]);`	436	`% imagesc(reshape(r(length(e0NormImCoords)+1:end), 2margin+1, 2margin+1), [-50 50]);`
436	`% drawnow;`	437	`% drawnow;`
437		438
438	`E(i,:) = x(1:3);`	439	`E(i,:) = x(1:3);`
439		440
440	`end`	441	`end`
441		442
442	`function stop = out(x, optimValues, state)`	443	`function stop = out(x, optimValues, state)`
443		444
444	`% r = optimValues.residual;`	445	`% r = optimValues.residual;`
445	`%`	446	`%`
446	`% figure;`	447	`% figure;`
447	`% subplot(1,2,1);`	448	`% subplot(1,2,1);`
448	`% imagesc(reshape(r(1:length(e0NormImCoords)), 2margin+1, 2margin+1), [-50 50]);`	449	`% imagesc(reshape(r(1:length(e0NormImCoords)), 2margin+1, 2margin+1), [-50 50]);`
449	`% subplot(1,2,2);`	450	`% subplot(1,2,2);`
450	`% imagesc(reshape(r(length(e0NormImCoords)+1:end), 2margin+1, 2margin+1), [-50 50]);`	451	`% imagesc(reshape(r(length(e0NormImCoords)+1:end), 2margin+1, 2margin+1), [-50 50]);`
451	`% drawnow;`	452	`% drawnow;`
452	`%`	453	`%`
453	`% display(x);`	454	`% display(x);`
454		455
455	`stop = false;`	456	`stop = false;`
456	`end`	457	`end`
457		458
458	`function r = circleResiduals(x)`	459	`function r = circleResiduals(x)`
459		460
460	`p0 = x(1:3);`	461	`p0 = x(1:3);`
461	`p1 = x(1:3) * camStereoParams.RotationOfCamera2 + camStereoParams.TranslationOfCamera2;`	462	`p1 = x(1:3) * camStereoParams.RotationOfCamera2 + camStereoParams.TranslationOfCamera2;`
462	`n0 = [x(4:5) 1.0];`	463	`n0 = [x(4:5) 1.0];`
463	`n0 = 1.0/norm(n0) * n0;`	464	`n0 = 1.0/norm(n0) * n0;`
464	`n1 = n0 * camStereoParams.RotationOfCamera2;`	465	`n1 = n0 * camStereoParams.RotationOfCamera2;`
465		466
466	`r = zeros(length(e0NormImCoords) + length(e1NormImCoords), 1);`	467	`r = zeros(length(e0NormImCoords) + length(e1NormImCoords), 1);`
467		468
468	`% norminal radius of markers`	469	`% norminal radius of markers`
469	`markerRadius = 1.4/2.0; %mm`	470	`markerRadius = 1.4/2.0; %mm`
470		471
471	`% half-width of ramp`	472	`% half-width of ramp`
472	`w = 0.1; %mm`	473	`w = 0.1; %mm`
473		474
474	`% camera 0`	475	`% camera 0`
475	`t = (p0n0')./(e0Homn0');`	476	`t = (p0n0')./(e0Homn0');`
476		477
477	`dVec = repmat(p0, nCoords, 1) - t.*e0Hom;`	478	`dVec = repmat(p0, nCoords, 1) - t.*e0Hom;`
478	`d = sqrt(sum(dVec.*dVec, 2));`	479	`d = sqrt(sum(dVec.*dVec, 2));`
479		480
480	`% "saturated" ramp function for marker disc shape`	481	`% "saturated" ramp function for marker disc shape`
481	`weights = max(min(1.0, -1.0/(2w)(d-markerRadius)+0.5), 0.0);`	482	`weights = max(min(1.0, -1.0/(2w)(d-markerRadius)+0.5), 0.0);`
482		483
483	`r(1:nCoords) = x(6)*weights - imVals0;`	484	`r(1:nCoords) = x(6)*weights - imVals0;`
484		485
485	`% camera 1`	486	`% camera 1`
486	`t = (p1n1')./(e1Homn1');`	487	`t = (p1n1')./(e1Homn1');`
487		488
488	`dVec = repmat(p1, nCoords, 1) - t.*e1Hom;`	489	`dVec = repmat(p1, nCoords, 1) - t.*e1Hom;`
489	`d = sqrt(sum(dVec.*dVec, 2));`	490	`d = sqrt(sum(dVec.*dVec, 2));`
490		491
491	`% "saturated" ramp function for marker disc shape`	492	`% "saturated" ramp function for marker disc shape`
492	`weights = max(min(1.0, -1.0/(2w)(d-markerRadius)+0.5), 0.0);`	493	`weights = max(min(1.0, -1.0/(2w)(d-markerRadius)+0.5), 0.0);`
493		494
494	`r(nCoords+1:end) = x(7)*weights - imVals1;`	495	`r(nCoords+1:end) = x(7)*weights - imVals1;`
495		496
496	`% figure;`	497	`% figure;`
497	`% subplot(1,2,1);`	498	`% subplot(1,2,1);`
498	`% imagesc(reshape(r(1:length(e0NormImCoords)), 2margin+1, 2margin+1), [-50 50]);`	499	`% imagesc(reshape(r(1:length(e0NormImCoords)), 2margin+1, 2margin+1), [-50 50]);`
499	`% subplot(1,2,2);`	500	`% subplot(1,2,2);`

Subversion Repositories seema-scanner

(root)/matlab/alignSubScansMarkers.m/ – Rev 243 → 246