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);
 
% 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
%for i=5:5
    % 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;
    
    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 < 1^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);
    
    % 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
    % fix Ri to be orthogonal
    [U,~,V] = svd(initialAlign(i).Rotation);
    Ri = U*V';
    
    % 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);
   
    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';
    Ti = alignment(i).Translation;
    
    Ea = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);
    
    % bring point cloud into optimized alignment
    pc = pcread(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(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 = 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 = (sqDistsSorted < 4.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 236	Rev 237
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		12
12	`calibration = readOpenCVXML(calibrationFileName);`	13	`calibration = readOpenCVXML(calibrationFileName);`
13		14
14	`% correct for Matlab 1-indexing in principle point coordinates`	15	`% correct for Matlab 1-indexing in principle point coordinates`
15	`calibration.K0(1:2, 3) = calibration.K0(1:2, 3)+1;`	16	`calibration.K0(1:2, 3) = calibration.K0(1:2, 3)+1;`
16	`calibration.K1(1:2, 3) = calibration.K1(1:2, 3)+1;`	17	`calibration.K1(1:2, 3) = calibration.K1(1:2, 3)+1;`
17		18
18	`% full projection matrices in Matlab convention`	19	`% full projection matrices in Matlab convention`
19	`P0 = transpose(calibration.K0*[eye(3) zeros(3,1)]);`	20	`P0 = transpose(calibration.K0*[eye(3) zeros(3,1)]);`
20	`P1 = transpose(calibration.K1*[calibration.R1 calibration.T1']);`	21	`P1 = transpose(calibration.K1*[calibration.R1 calibration.T1']);`
21		22
22	`% matlab cam params for undistortion`	23	`% matlab cam params for undistortion`
23	`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]));`
24	`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]));`
25		26
26	`% matlab struct for triangulation`	27	`% matlab struct for triangulation`
27	`camStereoParams = stereoParameters(camParams0, camParams1, calibration.R1', calibration.T1');`	28	`camStereoParams = stereoParameters(camParams0, camParams1, calibration.R1', calibration.T1');`
28		29
29	`nSubScans = length(initialAlign);`	30	`nSubScans = length(initialAlign);`
30		31
31	`% 3D coordinates of markers in local camera frame`	32	`% 3D coordinates of markers in local camera frame`
32	`E = cell(nSubScans, 1);`	33	`E = cell(nSubScans, 1);`
33		34
34	`% 3D coordinates of markers in global initial alignment`	35	`% 3D coordinates of markers in global initial alignment`
35	`Eg = cell(size(E));`	36	`Eg = cell(size(E));`
36		37
37	`% find 3D markers coordinates`	38	`% find 3D markers coordinates`
38	`for i=1:nSubScans`	39	`for i=1:nSubScans`
39	`%for i=5:5`	40	`%for i=5:5`
40	`% load point cloud`	41	`% load point cloud`
-		42	`pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);`
-		43	`pcFilePath = fileparts(pcFileName);`
41	`pc = pcread(initialAlign(i).FileName);`	44	`pc = pcread(pcFileName);`
42	`Q = pc.Location;`	45	`Q = pc.Location;`
43	`idString = strsplit(initialAlign(i).FileName, {'.ply', '_'});`	46	`idString = strsplit(initialAlign(i).FileName, {'.ply', '_'});`
44	`idString = idString{end-1};`	47	`idString = idString{end-1};`
45		48
46	`% load white frames`	49	`% load white frames`
-		50	`frame0 = imread(fullfile(pcFilePath, ['sequence_' idString], 'frames0_0.png'));`
-		51	`frame1 = imread(fullfile(pcFilePath, ['sequence_' idString], 'frames1_0.png'));`
-		52
-		53	`e0Coords = autoDetectMarkers(frame0);`
-		54	`e1Coords = autoDetectMarkers(frame1);`
-		55
47	`scanDir = strsplit(initialAlign(i).FileName, '/');`	56	`%e0Coords = manuallyDetectMarkers(frame0);`
48	`scanDir = fullfile(scanDir{1:end-1});`	57	`%e1Coords = manuallyDetectMarkers(frame1);`
-		58
49	`frame0 = imread(fullfile(scanDir, ['sequence_' idString], 'frames0_0.png'));`	59	`%[e0Coords, conf0] = detectMarkersSubpix(frame0, e0Coords, P0, Q);`
50	`frame1 = imread(fullfile(scanDir, ['sequence_' idString], 'frames1_0.png'));`	60	`%[e1Coords, conf1] = detectMarkersSubpix(frame1, e1Coords, P1, Q);`
51		61
52	`e0Coords = autoDetectMarkers(frame0, P0, Q);`	62	`if(length(e0Coords) < 1 \|\| length(e1Coords) < 1)`
53	`e1Coords = autoDetectMarkers(frame1, P1, Q);`	63	`continue;`
-		64	`end`
54		65
-		66	`% figure;`
-		67	`% subplot(1,2,1);`
-		68	`% imshow(frame0);`
-		69	`% hold('on');`
55	`%e0Coords = manuallyDetectMarkers(frame0, P0, Q);`	70	`% plot(e0Coords(:,1), e0Coords(:,2), 'rx', 'MarkerSize', 15);`
-		71	`% subplot(1,2,2);`
-		72	`% imshow(frame1);`
-		73	`% hold('on');`
56	`%e1Coords = manuallyDetectMarkers(frame1, P1, Q);`	74	`% plot(e1Coords(:,1), e1Coords(:,2), 'rx', 'MarkerSize', 15);`
-		75	`% drawnow;`
57		76
58	`e0Coords = undistortPoints(e0Coords, camParams0);`	77	`e0Coords = undistortPoints(e0Coords, camParams0);`
59	`e1Coords = undistortPoints(e1Coords, camParams1);`	78	`e1Coords = undistortPoints(e1Coords, camParams1);`
60		79
61	`% match ellipse candidates between cameras based on projection`	80	`% match ellipse candidates between cameras based on projection`
62	`E0 = projectOntoPointCloud(e0Coords, P0, Q);`	81	`E0 = projectOntoPointCloud(e0Coords, P0, Q);`
63	`E1 = projectOntoPointCloud(e1Coords, P1, Q);`	82	`E1 = projectOntoPointCloud(e1Coords, P1, Q);`
64		83
65	`matchedPairs = nan(size(E0, 1), 2);`	84	`matchedPairs = nan(size(E0, 1), 2);`
66	`nMatchedPairs = 0;`	85	`nMatchedPairs = 0;`
67	`for j=1:size(E0, 1)`	86	`for j=1:size(E0, 1)`
68		87
69	`% should use pdist2 instead..`	88	`% should use pdist2 instead..`
70	`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);`
71		90
72	`[minSqDist, minSqDistIdx] = min(sqDists);`	91	`[minSqDist, minSqDistIdx] = min(sqDists);`
73		92
74	`if(minSqDist < 1^2)`	93	`if(minSqDist < 1^2)`
75	`nMatchedPairs = nMatchedPairs + 1;`	94	`nMatchedPairs = nMatchedPairs + 1;`
76	`matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];`	95	`matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];`
77	`end`	96	`end`
78	`end`	97	`end`
79	`matchedPairs = matchedPairs(1:nMatchedPairs, :);`	98	`matchedPairs = matchedPairs(1:nMatchedPairs, :);`
80		99
-		100	`figure;`
-		101	`subplot(1,2,1);`
-		102	`imshow(frame0);`
-		103	`hold('on');`
-		104	`plot(e0Coords(matchedPairs(:, 1),1), e0Coords(matchedPairs(:, 1),2), 'rx', 'MarkerSize', 15);`
-		105	`subplot(1,2,2);`
-		106	`imshow(frame1);`
-		107	`hold('on');`
-		108	`plot(e1Coords(matchedPairs(:, 2),1), e1Coords(matchedPairs(:, 2),2), 'rx', 'MarkerSize', 15);`
-		109	`drawnow;`
-		110
81	`% triangulate marker centers (lens correction has been performed)`	111	`% triangulate marker centers (lens correction has been performed)`
82	`[E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);`	112	`[E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);`
83	`E{i} = E{i}(e<3.0, :);`	113	`E{i} = E{i}(e<3.0, :);`
84	`display(e);`	114	`display(e);`
85		115
86	`% write point cloud with marker (debugging)`	116	`% write point cloud with marker (debugging)`
87	`pcDebug = pointCloud([pc.Location; E{i}], 'Color', [pc.Color; repmat([255, 0, 0], size(E{i}, 1), 1)]);`	117	`pcDebug = pointCloud([pc.Location; E{i}], 'Color', [pc.Color; repmat([255, 0, 0], size(E{i}, 1), 1)]);`
88	`pcwrite(pcDebug, 'pcDebug.ply');`	118	`pcwrite(pcDebug, 'pcDebug.ply');`
89		119
90	`% bring markers into initial alignment`	120	`% bring markers into initial alignment`
91	`[U,~,V] = svd(initialAlign(i).Rotation);`	121	`[U,~,V] = svd(initialAlign(i).Rotation);`
92	`Ri = U*V';`	122	`Ri = U*V';`
93	`Ti = initialAlign(i).Translation;`	123	`Ti = initialAlign(i).Translation;`
94		124
95	`Eg{i} = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);`	125	`Eg{i} = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);`
96	`end`	126	`end`
97		127
98	`% show found markers in initial alignment`	128	`% show found markers in initial alignment`
99	`figure;`	129	`figure;`
100	`hold('on');`	130	`hold('on');`
101	`for i=1:nSubScans`	131	`for i=1:nSubScans`
102	`% fix Ri to be orthogonal`	132	`% fix Ri to be orthogonal`
103	`[U,~,V] = svd(initialAlign(i).Rotation);`	133	`[U,~,V] = svd(initialAlign(i).Rotation);`
104	`Ri = U*V';`	134	`Ri = U*V';`
105		135
106	`% bring point cloud into initial alignment`	136	`% bring point cloud into initial alignment`
-		137	`pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);`
107	`pc = pcread(initialAlign(i).FileName);`	138	`pc = pcread(pcFileName);`
108	`tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);`	139	`tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);`
109	`pcg = pctransform(pc, tform);`	140	`pcg = pctransform(pc, tform);`
110		141
111	`pcshow(pcg);`	142	`pcshow(pcg);`
112	`xlabel('x');`	143	`xlabel('x');`
113	`ylabel('y');`	144	`ylabel('y');`
114	`zlabel('z');`	145	`zlabel('z');`
115		146
116	`plot3(Eg{i}(:,1), Eg{i}(:,2), Eg{i}(:,3), '.', 'MarkerSize', 15);`	147	`plot3(Eg{i}(:,1), Eg{i}(:,2), Eg{i}(:,3), '.', 'MarkerSize', 15);`
117	`title('Initial Alignment');`	148	`title('Initial Alignment');`
118	`end`	149	`end`
119		150
120	`% match markers between poses using initial alignment`	151	`% match markers between poses using initial alignment`
121	`Pg = {};`	152	`Pg = {};`
122	`P = {};`	153	`P = {};`
123	`for i=1:nSubScans`	154	`for i=1:nSubScans`
124	`for j=1:size(Eg{i}, 1)`	155	`for j=1:size(Eg{i}, 1)`
125	`pg = Eg{i}(j,:);`	156	`pg = Eg{i}(j,:);`
126	`p = E{i}(j,:);`	157	`p = E{i}(j,:);`
127	`matched = false;`	158	`matched = false;`
128	`for k=1:size(Pg, 2)`	159	`for k=1:size(Pg, 2)`
129	`clusterCenter = mean(cat(1, Pg{:,k}), 1);`	160	`clusterCenter = mean(cat(1, Pg{:,k}), 1);`
130	`if(sum((pg - clusterCenter).^2) < 3^2)`	161	`if(sum((pg - clusterCenter).^2) < 3^2)`
131	`% store in global frame`	162	`% store in global frame`
132	`Pg{i,k} = pg;`	163	`Pg{i,k} = pg;`
133	`% store in local frame`	164	`% store in local frame`
134	`P{i,k} = p;`	165	`P{i,k} = p;`
135	`matched = true;`	166	`matched = true;`
136	`break;`	167	`break;`
137	`end`	168	`end`
138	`end`	169	`end`
139	`% create new cluster`	170	`% create new cluster`
140	`if(not(matched))`	171	`if(not(matched))`
141	`Pg{i,end+1} = pg;`	172	`Pg{i,end+1} = pg;`
142	`P{i,end+1} = p;`	173	`P{i,end+1} = p;`
143	`end`	174	`end`
144	`end`	175	`end`
145	`end`	176	`end`
146		177
147	`% run optimization`	178	`% run optimization`
148	`alignment = groupwiseOrthogonalProcrustes(P, initialAlign);`	179	`alignment = groupwiseOrthogonalProcrustes(P, initialAlign);`
149		180
150	`% show found markers in optimized alignment`	181	`% show found markers in optimized alignment`
151	`figure;`	182	`figure;`
152	`hold('on');`	183	`hold('on');`
153	`for i=1:nSubScans`	184	`for i=1:nSubScans`
154	`% fix Ri to be orthogonal`	185	`% fix Ri to be orthogonal`
155	`[U,~,V] = svd(alignment(i).Rotation);`	186	`[U,~,V] = svd(alignment(i).Rotation);`
156	`Ri = U*V';`	187	`Ri = U*V';`
157	`Ti = alignment(i).Translation;`	188	`Ti = alignment(i).Translation;`
158		189
159	`Ea = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);`	190	`Ea = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);`
160		191
161	`% bring point cloud into optimized alignment`	192	`% bring point cloud into optimized alignment`
162	`pc = pcread(initialAlign(i).FileName);`	193	`pc = pcread(initialAlign(i).FileName);`
163	`tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);`	194	`tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);`
164	`pcg = pctransform(pc, tform);`	195	`pcg = pctransform(pc, tform);`
165		196
166	`pcshow(pcg);`	197	`pcshow(pcg);`
167	`xlabel('x');`	198	`xlabel('x');`
168	`ylabel('y');`	199	`ylabel('y');`
169	`zlabel('z');`	200	`zlabel('z');`
170		201
171	`plot3(Ea(:,1), Ea(:,2), Ea(:,3), '.', 'MarkerSize', 15);`	202	`plot3(Ea(:,1), Ea(:,2), Ea(:,3), '.', 'MarkerSize', 15);`
172	`title('Optimized Alignment');`	203	`title('Optimized Alignment');`
173	`end`	204	`end`
174		205
175	`% write to ALN file`	206	`% write to ALN file`
176	`for i=1:length(alignment)`	207	`for i=1:length(alignment)`
177	`alignment(i).FileName = initialAlign(i).FileName;`	208	`alignment(i).FileName = initialAlign(i).FileName;`
178	`end`	209	`end`
179		210
180	`writeMeshLabALN(alignment, strrep(alnFileName, '.aln', 'Optimized.aln'));`	211	`writeMeshLabALN(alignment, strrep(alnFileName, '.aln', 'Optimized.aln'));`
181		212
182	`end`	213	`end`
183		214
184	`function e = autoDetectMarkers(frame, P, pointCloud)`	215	`function e = autoDetectMarkers(frame, P, pointCloud)`
185		216
186	`% create mask based on morphology`	217	`% create mask based on morphology`
187	`g = rgb2gray(frame);`	218	`g = rgb2gray(frame);`
188	`g(g>254) = 0;`	219	`% g(g>254) = 0;`
189	`bw = imbinarize(g, 'adaptive', 'Sensitivity', 10^(-50));`	220	`% bw = imbinarize(g, 'adaptive', 'Sensitivity', 10^(-50));`
-		221	`bw = imbinarize(g, 0.10);`
190	`cc = bwconncomp(bw);`	222	`cc = bwconncomp(bw);`
191	`rp = regionprops(cc, 'Area', 'Solidity', 'Eccentricity', 'Centroid');`	223	`rp = regionprops(cc, 'Area', 'Solidity', 'Eccentricity', 'Centroid');`
192	`idx = ([rp.Area] > 100 & [rp.Area] < 1000 & [rp.Solidity] > 0.9);`	224	`idx = ([rp.Area] > 100 & [rp.Area] < 1000 & [rp.Solidity] > 0.9);`
193		225
194	`initialGuesses = cat(1, rp(idx).Centroid);`	226	`e = cat(1, rp(idx).Centroid);`
195		227
196	`[e, ~] = detectMarkersSubpix(frame, initialGuesses, P, pointCloud);`	-
197		-
198	`figure;`	-
199	`imshow(frame);`	-
200	`hold('on');`	-
201	`plot(e(:,1), e(:,2), 'rx', 'MarkerSize', 15);`	-
202	`drawnow;`	-
203	`end`	228	`end`
204		229
205	`function e = manuallyDetectMarkers(frame, P, pointCloud)`	230	`function e = manuallyDetectMarkers(frame, P, pointCloud)`
206		231
207	`e = [];`	232	`e = [];`
208	`%edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);`	233	`%edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);`
209		234
210	`figure;`	235	`figure;`
211	`hold('on');`	236	`hold('on');`
212	`imshow(frame);`	237	`imshow(frame);`
213	`title('Close figure to end.');`	238	`title('Close figure to end.');`
214	`set(gcf, 'pointer', 'crosshair');`	239	`set(gcf, 'pointer', 'crosshair');`
215	`set(gcf, 'WindowButtonDownFcn', @clickCallback);`	240	`set(gcf, 'WindowButtonDownFcn', @clickCallback);`
216		241
217	`uiwait;`	242	`uiwait;`
218		243
219	`function clickCallback(caller, ~)`	244	`function clickCallback(caller, ~)`
220		245
221	`p = get(gca, 'CurrentPoint');`	246	`p = get(gca, 'CurrentPoint');`
222	`p = p(1, 1:2);`	247	`p = p(1, 1:2);`
223		248
224	`[el, ~] = detectMarkersSubpix(frame, p, P, pointCloud);`	-
225	`e = [e; el(:, 1:2)];`	249	`e = [e; p(:, 1:2)];`
226		250
227	`if(not(isempty(el)))`	251	`if(not(isempty(el)))`
228	`figure(caller);`	252	`figure(caller);`
229	`hold('on');`	253	`hold('on');`
230	`plot(el(1), el(2), 'rx', 'MarkerSize', 15);`	254	`plot(el(1), el(2), 'rx', 'MarkerSize', 15);`
231	`end`	255	`end`
232	`end`	256	`end`
233		257
234	`end`	258	`end`
235		259
236	`function [e, conf] = detectMarkersSubpix(frame, initGuesses, P, Q)`	260	`function [e, conf] = detectMarkersSubpix(frame, initGuesses, P, Q)`
-		261	`% Detect a marker in a single frame by rectifying to the image and`
-		262	`% performing image registration.`
237		263
238	`% create mask based on morphology`	264	`% create mask based on morphology`
239	`g = rgb2gray(frame);`	265	`g = rgb2gray(frame);`
240	`g(g>254) = 0;`	266	`g(g>254) = 0;`
241	`bw = imbinarize(g);`	267	`bw = imbinarize(g);`
242	`cc = bwconncomp(bw);`	268	`cc = bwconncomp(bw);`
243	`labels = labelmatrix(cc);`	269	`labels = labelmatrix(cc);`
244		270
245	`% project point cloud into image`	271	`% project point cloud into image`
246	`q = [Q ones(size(Q,1),1)]*P;`	272	`q = [Q ones(size(Q,1),1)]*P;`
247	`q = q./[q(:,3) q(:,3) q(:,3)];`	273	`q = q./[q(:,3) q(:,3) q(:,3)];`
248		274
249	`e = zeros(size(initGuesses));`	275	`e = zeros(size(initGuesses));`
250	`conf = zeros(size(initGuesses, 1), 1);`	276	`conf = zeros(size(initGuesses, 1), 1);`
251		277
252	`nMarkersFound = 0;`	278	`nMarkersFound = 0;`
253		279
254	`for i=1:size(initGuesses, 1)`	280	`for i=1:size(initGuesses, 1)`
255		281
256	`labelId = labels(round(initGuesses(i,2)), round(initGuesses(i,1)));`	282	`labelId = labels(round(initGuesses(i,2)), round(initGuesses(i,1)));`
257	`labelMask = (labels == labelId);`	283	`labelMask = (labels == labelId);`
258	`labelMask = imdilate(labelMask, strel('disk', 3, 0));`	284	`labelMask = imdilate(labelMask, strel('disk', 3, 0));`
259		285
260	`if(sum(sum(labelMask)) < 10 \|\| sum(sum(labelMask)) > 1000)`	286	`if(sum(sum(labelMask)) < 10 \|\| sum(sum(labelMask)) > 1000)`
261	`continue;`	287	`continue;`
262	`end`	288	`end`
263		289
264	`% determine 3D points that are part of the marker`	290	`% determine 3D points that are part of the marker`
265	`% note: we should probably undistort labelMask`	291	`% note: we should probably undistort labelMask`
266	`pointMask = false(size(q, 1), 1);`	292	`pointMask = false(size(q, 1), 1);`
267	`for j=1:size(q,1)`	293	`for j=1:size(q,1)`
268	`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)`	294	`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)`
269	`continue;`	295	`continue;`
270	`end`	296	`end`
271		297
272	`if(labelMask(round(q(j,2)), round(q(j,1))))`	298	`if(labelMask(round(q(j,2)), round(q(j,1))))`
273	`pointMask(j) = true;`	299	`pointMask(j) = true;`
274	`end`	300	`end`
275	`end`	301	`end`
276		302
277	`if(sum(pointMask)) < 10`	303	`if(sum(pointMask)) < 10`
278	`continue;`	304	`continue;`
279	`end`	305	`end`
280		306
281	`% project 3D points onto local plane`	307	`% project 3D points onto local plane`
282	`[~, sc, ~] = pca(Q(pointMask, :));`	308	`[~, sc, ~] = pca(Q(pointMask, :));`
283	`Qlocal = sc(:, 1:2);`	309	`Qlocal = sc(:, 1:2);`
284		310
285	`% synthetic marker in high res. space`	311	`% synthetic marker in high res. space`
286	`m = zeros(151, 151);`	312	`m = zeros(151, 151);`
287	`[x, y] = meshgrid(1:151, 1:151);`	313	`[x, y] = meshgrid(1:151, 1:151);`
288	`m((x(:)-76).^2 + (y(:)-76).^2 <= 50^2) = 1.0;`	314	`m((x(:)-76).^2 + (y(:)-76).^2 <= 50^2) = 1.0;`
289		315
290	`% relation between marker space (px) and true marker/local plane(mm)`	316	`% relation between marker space (px) and true marker/local plane(mm)`
291	`% true marker diameter is 1.75mm`	317	`% true marker diameter is 1.75mm`
292	`mScale = 101/1.4; %px/mm`	318	`mScale = 101/1.4; %px/mm`
293	`mShift = 76; %px`	319	`mShift = 76; %px`
294		320
295	`% build homography from image to marker space`	321	`% build homography from image to marker space`
296	`H = fitgeotrans(q(pointMask, 1:2), mScale*Qlocal+mShift, 'projective');`	322	`H = fitgeotrans(q(pointMask, 1:2), mScale*Qlocal+mShift, 'projective');`
297	`%Hdlt = Hest_DLT([mScale*Qlocal+mShift, ones(size(Qlocal, 1), 1)]', q(pointMask,:)');`	323	`%Hdlt = Hest_DLT([mScale*Qlocal+mShift, ones(size(Qlocal, 1), 1)]', q(pointMask,:)');`
298	`%H = projective2d(Hdlt');`	324	`%H = projective2d(Hdlt');`
299		325
300	`% bring image of marker into marker space`	326	`% bring image of marker into marker space`
301	`imMarkerSpace = imwarp(frame, H, 'OutputView', imref2d(size(m)));`	327	`imMarkerSpace = imwarp(frame, H, 'OutputView', imref2d(size(m)));`
302	`imMarkerSpace = rgb2gray(im2double(imMarkerSpace));`	328	`imMarkerSpace = rgb2gray(im2double(imMarkerSpace));`
303		329
304	`%figure; imshowpair(imMarkerSpace, m);`	330	`%figure; imshowpair(imMarkerSpace, m);`
305		331
306	`% perform image registration`	332	`% perform image registration`
307	`% might be better off using subpixel image correlation`	333	`% might be better off using subpixel image correlation`
308	`[opt, met] = imregconfig('multimodal');`	334	`[opt, met] = imregconfig('multimodal');`
309	`T = imregtform(m, imMarkerSpace, 'translation', opt, met, 'DisplayOptimization', false);`	335	`T = imregtform(m, imMarkerSpace, 'translation', opt, met, 'DisplayOptimization', false);`
310		336
311	`rege = imwarp(m, T, 'OutputView', imref2d(size(m)));`	337	`rege = imwarp(m, T, 'OutputView', imref2d(size(m)));`
312	`%figure; imshowpair(imMarkerSpace, rege);`	338	`%figure; imshowpair(imMarkerSpace, rege);`
313		339
314	`% measure of correlation`	340	`% measure of correlation`
315	`confI = sum(sum(imMarkerSpace .* rege))/sqrt(sum(sum(imMarkerSpace) * sum(sum(rege))));`	341	`confI = sum(sum(imMarkerSpace .* rege))/sqrt(sum(sum(imMarkerSpace) * sum(sum(rege))));`
316	`%confI = 1.0;`	342	`%confI = 1.0;`
317		343
318	`if confI<0.4`	344	`if confI<0.4`
319	`continue;`	345	`continue;`
320	`end`	346	`end`
321		347
322	`fprintf('Found marker with confidence: %f\n', confI);`	348	`fprintf('Found marker with confidence: %f\n', confI);`
323		349
324	`% transform marker space coordinates (76,76) to frame space`	350	`% transform marker space coordinates (76,76) to frame space`
325	`el = T.transformPointsForward([76, 76]);`	351	`el = T.transformPointsForward([76, 76]);`
326	`el = H.transformPointsInverse(el);`	352	`el = H.transformPointsInverse(el);`
327		353
328	`nMarkersFound = nMarkersFound+1;`	354	`nMarkersFound = nMarkersFound+1;`
329	`e(nMarkersFound,:) = el;`	355	`e(nMarkersFound,:) = el;`
330	`conf(nMarkersFound) = confI;`	356	`conf(nMarkersFound) = confI;`
331	`end`	357	`end`
332		358
333	`e = e(1:nMarkersFound, :);`	359	`e = e(1:nMarkersFound, :);`
334	`conf = conf(1:nMarkersFound);`	360	`conf = conf(1:nMarkersFound);`
335	`end`	361	`end`
336		362
337	`function E = projectOntoPointCloud(e, P, pointCloud)`	363	`function E = projectOntoPointCloud(e, P, pointCloud)`
-		364	`% Project 2d point coordinates onto pointCloud to find corresponding 3d`
-		365	`% point coordinates.`
338		366
339	`q = [pointCloud ones(size(pointCloud,1),1)]*P;`	367	`q = [pointCloud ones(size(pointCloud,1),1)]*P;`
340	`q = q(:,1:2)./[q(:,3) q(:,3)];`	368	`q = q(:,1:2)./[q(:,3) q(:,3)];`
341		369
342	`E = nan(size(e,1), 3);`	370	`E = nan(size(e,1), 3);`
343		371
344	`for i=1:size(e, 1)`	372	`for i=1:size(e, 1)`
345	`sqDists = sum((q - repmat(e(i,:), size(q, 1), 1)).^2, 2);`	373	`sqDists = sum((q - repmat(e(i,:), size(q, 1), 1)).^2, 2);`
346		374
347	`[sqDistsSorted, sortIdx] = sort(sqDists);`	375	`[sqDistsSorted, sortIdx] = sort(sqDists);`
348		376
349	`neighbors = (sqDistsSorted < 4.0^2);`	377	`neighbors = (sqDistsSorted < 4.0^2);`
350		378
351	`distsSorted = sqrt(sqDistsSorted(neighbors));`	379	`distsSorted = sqrt(sqDistsSorted(neighbors));`
352	`invDistsSorted = 1.0/distsSorted;`	380	`invDistsSorted = 1.0/distsSorted;`
353	`sortIdx = sortIdx(neighbors);`	381	`sortIdx = sortIdx(neighbors);`
354		382
355	`nNeighbors = sum(neighbors);`	383	`nNeighbors = sum(neighbors);`
356		384
357	`if(nNeighbors >= 2)`	385	`if(nNeighbors >= 2)`
358	`E(i, :) = 0;`	386	`E(i, :) = 0;`
359	`for j=1:nNeighbors`	387	`for j=1:nNeighbors`
360	`E(i, :) = E(i, :) + invDistsSorted(j)/sum(invDistsSorted) * pointCloud(sortIdx(j), :);`	388	`E(i, :) = E(i, :) + invDistsSorted(j)/sum(invDistsSorted) * pointCloud(sortIdx(j), :);`
361	`end`	389	`end`
362	`end`	390	`end`
363		391
364	`end`	392	`end`
365	`end`	393	`end`
366		394
367	`function H = Hest_DLT(q1, q2)`	395	`function H = Hest_DLT(q1, q2)`
368	`% Estimate the homography between a set of point correspondences using the`	396	`% Estimate the homography between a set of point correspondences using the`
369	`% direct linear transform algorithm.`	397	`% direct linear transform algorithm.`
370	`%`	398	`%`
371	`% Input:`	399	`% Input:`
372	`% q1: 3xN matrix of homogenous point coordinates from camera 1.`	400	`% q1: 3xN matrix of homogenous point coordinates from camera 1.`
373	`% q2: 3xN matrix of corresponding points from camera 2.`	401	`% q2: 3xN matrix of corresponding points from camera 2.`
374	`% Output:`	402	`% Output:`
375	`% H: 3x3 matrix. The Fundamental Matrix estimate.`	403	`% H: 3x3 matrix. The Fundamental Matrix estimate.`
376	`%`	404	`%`
377	`% Note that N must be at least 4.`	405	`% Note that N must be at least 4.`
378	`% See derivation in Aanaes, Lecture Notes on Computer Vision, 2011`	406	`% See derivation in Aanaes, Lecture Notes on Computer Vision, 2011`
379		407
380	`% Normalize points`	408	`% Normalize points`
381	`[T1,invT1] = normalizationMat(q1);`	409	`[T1,invT1] = normalizationMat(q1);`
382	`q1_tilde = T1*q1;`	410	`q1_tilde = T1*q1;`
383		411
384	`T2 = normalizationMat(q2);`	412	`T2 = normalizationMat(q2);`
385	`q2_tilde = T2*q2;`	413	`q2_tilde = T2*q2;`
386		414
387	`% DLT estimation`	415	`% DLT estimation`
388	`N = size(q1_tilde,2);`	416	`N = size(q1_tilde,2);`
389	`assert(size(q2_tilde,2)==N);`	417	`assert(size(q2_tilde,2)==N);`
390		418
391	`B = zeros(3*N,9);`	419	`B = zeros(3*N,9);`
392		420
393	`for i=1:N`	421	`for i=1:N`
394	`q1i = q1_tilde(:,i);`	422	`q1i = q1_tilde(:,i);`
395	`q2i = q2_tilde(:,i);`	423	`q2i = q2_tilde(:,i);`
396	`q1_x = [0 -q1i(3) q1i(2); q1i(3) 0 -q1i(1); -q1i(2) q1i(1) 0];`	424	`q1_x = [0 -q1i(3) q1i(2); q1i(3) 0 -q1i(1); -q1i(2) q1i(1) 0];`
397	`biT = kron(q2i', q1_x);`	425	`biT = kron(q2i', q1_x);`
398	`B(3(i-1)+1:3i, :) = biT;`	426	`B(3(i-1)+1:3i, :) = biT;`
399	`end`	427	`end`
400		428
401	`[U,S,~] = svd(B');`	429	`[U,S,~] = svd(B');`
402		430
403	`[~,idx] = min(diag(S));`	431	`[~,idx] = min(diag(S));`
404	`h = U(:,idx);`	432	`h = U(:,idx);`
405		433
406	`H_tilde = reshape(h, 3, 3);`	434	`H_tilde = reshape(h, 3, 3);`
407		435
408	`% Unnormalize H`	436	`% Unnormalize H`
409	`H = invT1H_tildeT2;`	437	`H = invT1H_tildeT2;`
410		438
411	`% Arbitrarily chose scale`	439	`% Arbitrarily chose scale`
412	`H = H * 1/H(3,3);`	440	`H = H * 1/H(3,3);`
413	`end`	441	`end`
414		442
415	`function [T,invT] = normalizationMat(q)`	443	`function [T,invT] = normalizationMat(q)`
416	`% Gives a normalization matrix for homogeneous coordinates`	444	`% Gives a normalization matrix for homogeneous coordinates`
417	`% such that T*q will have zero mean and unit variance.`	445	`% such that T*q will have zero mean and unit variance.`
418	`% See Aanaes, Computer Vision Lecture Notes 2.8.2`	446	`% See Aanaes, Computer Vision Lecture Notes 2.8.2`
419	`%`	447	`%`
420	`% q: (M+1)xN matrix of N MD points in homogenous coordinates`	448	`% q: (M+1)xN matrix of N MD points in homogenous coordinates`
421	`%`	449	`%`
422	`% Extended to also efficiently compute the inverse matrix`	450	`% Extended to also efficiently compute the inverse matrix`
423	`% DTU, 2013, Jakob Wilm`	451	`% DTU, 2013, Jakob Wilm`
424		452
425	`[M,N] = size(q);`	453	`[M,N] = size(q);`
426	`M = M-1;`	454	`M = M-1;`
427		455
428	`mu = mean(q(1:M,:),2);`	456	`mu = mean(q(1:M,:),2);`
429		457
430	`q_bar = q(1:M,:)-repmat(mu,1,N);`	458	`q_bar = q(1:M,:)-repmat(mu,1,N);`
431		459
432	`s = mean(sqrt(diag(q_bar'*q_bar)))/sqrt(2);`	460	`s = mean(sqrt(diag(q_bar'*q_bar)))/sqrt(2);`
433		461
434	`T = [eye(M)/s, -mu/s; zeros(1,M) 1];`	462	`T = [eye(M)/s, -mu/s; zeros(1,M) 1];`
435		463
436	`invT = [eye(M)*s, mu; zeros(1,M) 1];`	464	`invT = [eye(M)*s, mu; zeros(1,M) 1];`
437	`end`	465	`end`
438		466
439	`Generated by GNU Enscript 1.6.6.`	467	`Generated by GNU Enscript 1.6.6.`
440		468
441		469
442		470

Subversion Repositories seema-scanner

(root)/matlab/alignSubScansMarkers.m – Rev 236 → 237