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);
 
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
 
    % load point cloud
    pc = pcread(initialAlign(i).FileName);
    Q = pc.Location;
    idString = strsplit(initialAlign(i).FileName, {'.ply', '_'});
    idString = idString{end-1};
    
    % load white frames
    scanDir = strsplit(initialAlign(i).FileName, '/');
    scanDir = fullfile(scanDir{1:end-1});
    frame0 = imread(fullfile(scanDir, ['sequence_' idString], 'frames0_0.png'));
    frame1 = imread(fullfile(scanDir, ['sequence_' idString], 'frames1_0.png'));
 
    e0Coords = autoDetectMarkers(frame0, P0, Q);
    e1Coords = autoDetectMarkers(frame1, P1, Q);
    
    %e0Coords = manuallyDetectMarkers(frame0, P0, Q);
    %e1Coords = manuallyDetectMarkers(frame1, P1, Q);
    
    e0Coords = undistortPoints(e0Coords, camParams0);
    e1Coords = undistortPoints(e1Coords, camParams1);
 
    % match ellipse candidates between cameras based on projection
    E0 = projectOntoPointCloud(e0Coords, P0, Q);
    E1 = projectOntoPointCloud(e1Coords, P1, Q);
 
    matchedPairs = nan(size(E0, 1), 2);
    nMatchedPairs = 0;
    for j=1:size(E0, 1)
        
        % should use pdist2 instead..
        sqDists = sum((E1 - repmat(E0(j,:), size(E1, 1), 1)).^2, 2);
        
        [minSqDist, minSqDistIdx] = min(sqDists);
 
        if(minSqDist < 1^2)
            nMatchedPairs = nMatchedPairs + 1;
            matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];
        end
    end
    matchedPairs = matchedPairs(1:nMatchedPairs, :);
    
    % triangulate marker centers (lens correction has been performed)
    [E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);
    E{i} = E{i}(e<1.0, :);
    
    % 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
    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(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
    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
    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
    bw = imbinarize(rgb2gray(frame));
    cc = bwconncomp(bw);
    rp = regionprops(cc, 'Area', 'Solidity', 'Eccentricity', 'Centroid');
    idx = ([rp.Area] > 100 & [rp.Area] < 1000 & [rp.Solidity] > 0.9);
    
    initialGuesses = cat(1, rp(idx).Centroid);
 
    [e, ~] = detectMarkersSubpix(frame, initialGuesses, P, pointCloud);
 
    figure; 
    imshow(frame);
        hold('on');
    plot(e(:,1), e(:,2), 'rx', 'MarkerSize', 15);
    drawnow;
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);
 
        [el, ~] = detectMarkersSubpix(frame, p, P, pointCloud);
        e = [e; el(:, 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)
 
    % create mask based on morphology
    bw = imbinarize(rgb2gray(frame));
    cc = bwconncomp(bw);
    labels = labelmatrix(cc);
 
    % project point cloud into image
    q = [Q ones(size(Q,1),1)]*P;
    q = q./[q(:,3) q(:,3) q(:,3)];
    
    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.8; %px/mm
        mShift = 76; %px
        
        % build homography from image to marker space
        H = fitgeotrans(q(pointMask, 1:2), mScale*Qlocal+mShift,  'projective');
        
        % 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', true);
        
        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))));
        
        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)
 
    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
 
 

Rev 215	Rev 216
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		11
12	`calibration = readOpenCVXML(calibrationFileName);`	12	`calibration = readOpenCVXML(calibrationFileName);`
13		13
-		14	`% correct for Matlab 1-indexing in principle point coordinates`
-		15	`calibration.K0(1:2, 3) = calibration.K0(1:2, 3)+1;`
-		16	`calibration.K1(1:2, 3) = calibration.K1(1:2, 3)+1;`
-		17
14	`% full projection matrices in Matlab convention`	18	`% full projection matrices in Matlab convention`
15	`P0 = transpose(calibration.K0*[eye(3) zeros(3,1)]);`	19	`P0 = transpose(calibration.K0*[eye(3) zeros(3,1)]);`
16	`P1 = transpose(calibration.K1*[calibration.R1 calibration.T1']);`	20	`P1 = transpose(calibration.K1*[calibration.R1 calibration.T1']);`
17		21
18	`% matlab cam params for undistortion`	22	`% matlab cam params for undistortion`
19	`camParams0 = cameraParameters('IntrinsicMatrix', calibration.K0', 'RadialDistortion', calibration.k0([1 2 5]), 'TangentialDistortion', calibration.k0([3 4]));`	23	`camParams0 = cameraParameters('IntrinsicMatrix', calibration.K0', 'RadialDistortion', calibration.k0([1 2 5]), 'TangentialDistortion', calibration.k0([3 4]));`
20	`camParams1 = cameraParameters('IntrinsicMatrix', calibration.K1', 'RadialDistortion', calibration.k1([1 2 5]), 'TangentialDistortion', calibration.k1([3 4]));`	24	`camParams1 = cameraParameters('IntrinsicMatrix', calibration.K1', 'RadialDistortion', calibration.k1([1 2 5]), 'TangentialDistortion', calibration.k1([3 4]));`
21		25
22	`% matlab struct for triangulation`	26	`% matlab struct for triangulation`
23	`camStereoParams = stereoParameters(camParams0, camParams1, calibration.R1', calibration.T1');`	27	`camStereoParams = stereoParameters(camParams0, camParams1, calibration.R1', calibration.T1');`
24		28
25	`nSubScans = length(initialAlign);`	29	`nSubScans = length(initialAlign);`
26		30
27	`% 3D coordinates of markers in local camera frame`	31	`% 3D coordinates of markers in local camera frame`
28	`E = cell(nSubScans, 1);`	32	`E = cell(nSubScans, 1);`
29		33
30	`% 3D coordinates of markers in global initial alignment`	34	`% 3D coordinates of markers in global initial alignment`
31	`Eg = cell(size(E));`	35	`Eg = cell(size(E));`
32		36
33	`% find 3D markers coordinates`	37	`% find 3D markers coordinates`
34	`for i=1:nSubScans`	38	`for i=1:nSubScans`
35		39
36	`% load point cloud`	40	`% load point cloud`
37	`pc = pcread(initialAlign(i).FileName);`	41	`pc = pcread(initialAlign(i).FileName);`
38	`Q = pc.Location;`	42	`Q = pc.Location;`
39	`idString = strsplit(initialAlign(i).FileName, {'.ply', '_'});`	43	`idString = strsplit(initialAlign(i).FileName, {'.ply', '_'});`
40	`idString = idString{end-1};`	44	`idString = idString{end-1};`
41		45
42	`% load white frames`	46	`% load white frames`
43	`scanDir = strsplit(initialAlign(i).FileName, '/');`	47	`scanDir = strsplit(initialAlign(i).FileName, '/');`
44	`scanDir = fullfile(scanDir{1:end-1});`	48	`scanDir = fullfile(scanDir{1:end-1});`
45	`frame0 = imread(fullfile(scanDir, ['sequence_' idString], 'frames0_0.png'));`	49	`frame0 = imread(fullfile(scanDir, ['sequence_' idString], 'frames0_0.png'));`
46	`frame1 = imread(fullfile(scanDir, ['sequence_' idString], 'frames1_0.png'));`	50	`frame1 = imread(fullfile(scanDir, ['sequence_' idString], 'frames1_0.png'));`
47		51
48	`e0Coords = autoDetectMarkers(frame0, P0, Q);`	52	`e0Coords = autoDetectMarkers(frame0, P0, Q);`
49	`e1Coords = autoDetectMarkers(frame1, P1, Q);`	53	`e1Coords = autoDetectMarkers(frame1, P1, Q);`
50		54
51	`%e0Coords = manuallyDetectMarkers(frame0, P0, Q);`	55	`%e0Coords = manuallyDetectMarkers(frame0, P0, Q);`
52	`%e1Coords = manuallyDetectMarkers(frame1, P1, Q);`	56	`%e1Coords = manuallyDetectMarkers(frame1, P1, Q);`
53		57
54	`e0Coords = undistortPoints(e0Coords, camParams0);`	58	`e0Coords = undistortPoints(e0Coords, camParams0);`
55	`e1Coords = undistortPoints(e1Coords, camParams1);`	59	`e1Coords = undistortPoints(e1Coords, camParams1);`
56		60
57	`% match ellipse candidates between cameras based on projection`	61	`% match ellipse candidates between cameras based on projection`
58	`E0 = projectOntoPointCloud(e0Coords, P0, Q);`	62	`E0 = projectOntoPointCloud(e0Coords, P0, Q);`
59	`E1 = projectOntoPointCloud(e1Coords, P1, Q);`	63	`E1 = projectOntoPointCloud(e1Coords, P1, Q);`
60		64
61	`matchedPairs = nan(size(E0, 1), 2);`	65	`matchedPairs = nan(size(E0, 1), 2);`
62	`nMatchedPairs = 0;`	66	`nMatchedPairs = 0;`
63	`for j=1:size(E0, 1)`	67	`for j=1:size(E0, 1)`
64		68
65	`% should use pdist2 instead..`	69	`% should use pdist2 instead..`
66	`sqDists = sum((E1 - repmat(E0(j,:), size(E1, 1), 1)).^2, 2);`	70	`sqDists = sum((E1 - repmat(E0(j,:), size(E1, 1), 1)).^2, 2);`
67		71
68	`[minSqDist, minSqDistIdx] = min(sqDists);`	72	`[minSqDist, minSqDistIdx] = min(sqDists);`
69		73
70	`if(minSqDist < 5^2)`	74	`if(minSqDist < 1^2)`
71	`nMatchedPairs = nMatchedPairs + 1;`	75	`nMatchedPairs = nMatchedPairs + 1;`
72	`matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];`	76	`matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];`
73	`end`	77	`end`
74	`end`	78	`end`
75	`matchedPairs = matchedPairs(1:nMatchedPairs, :);`	79	`matchedPairs = matchedPairs(1:nMatchedPairs, :);`
76		80
77	`% triangulate marker centers (lens correction has been performed)`	81	`% triangulate marker centers (lens correction has been performed)`
78	`[E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);`	82	`[E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);`
79	`E{i} = E{i}(e<0.5, :);`	83	`E{i} = E{i}(e<1.0, :);`
-		84
-		85	`% write point cloud with marker (debugging)`
-		86	`pcDebug = pointCloud([pc.Location; E{i}], 'Color', [pc.Color; repmat([255, 0, 0], size(E{i}, 1), 1)]);`
-		87	`pcwrite(pcDebug, 'pcDebug.ply');`
80		88
81	`% bring into initial alignment`	89	`% bring markers into initial alignment`
82	`[U,~,V] = svd(initialAlign(i).Rotation);`	90	`[U,~,V] = svd(initialAlign(i).Rotation);`
83	`Ri = U*V';`	91	`Ri = U*V';`
84	`Ti = initialAlign(i).Translation;`	92	`Ti = initialAlign(i).Translation;`
85		93
86	`Eg{i} = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);`	94	`Eg{i} = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);`
87	`end`	95	`end`
88		96
89	`% show found markers in initial alignment`	97	`% show found markers in initial alignment`
90	`figure;`	98	`figure;`
91	`hold('on');`	99	`hold('on');`
92	`for i=1:nSubScans`	100	`for i=1:nSubScans`
93	`% fix Ri to be orthogonal`	101	`% fix Ri to be orthogonal`
94	`[U,~,V] = svd(initialAlign(i).Rotation);`	102	`[U,~,V] = svd(initialAlign(i).Rotation);`
95	`Ri = U*V';`	103	`Ri = U*V';`
96		104
97	`% bring point cloud into initial alignment`	105	`% bring point cloud into initial alignment`
98	`pc = pcread(initialAlign(i).FileName);`	106	`pc = pcread(initialAlign(i).FileName);`
99	`tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);`	107	`tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);`
100	`pcg = pctransform(pc, tform);`	108	`pcg = pctransform(pc, tform);`
101		109
102	`pcshow(pcg);`	110	`pcshow(pcg);`
103	`xlabel('x');`	111	`xlabel('x');`
104	`ylabel('y');`	112	`ylabel('y');`
105	`zlabel('z');`	113	`zlabel('z');`
106		114
107	`plot3(Eg{i}(:,1), Eg{i}(:,2), Eg{i}(:,3), '.', 'MarkerSize', 15);`	115	`plot3(Eg{i}(:,1), Eg{i}(:,2), Eg{i}(:,3), '.', 'MarkerSize', 15);`
108	`title('Initial Alignment');`	116	`title('Initial Alignment');`
109	`end`	117	`end`
110		118
111	`% match markers between poses using initial alignment`	119	`% match markers between poses using initial alignment`
112	`Pg = {};`	120	`Pg = {};`
113	`P = {};`	121	`P = {};`
114	`for i=1:nSubScans`	122	`for i=1:nSubScans`
115	`for j=1:size(Eg{i}, 1)`	123	`for j=1:size(Eg{i}, 1)`
116	`pg = Eg{i}(j,:);`	124	`pg = Eg{i}(j,:);`
117	`p = E{i}(j,:);`	125	`p = E{i}(j,:);`
118	`matched = false;`	126	`matched = false;`
119	`for k=1:size(Pg, 2)`	127	`for k=1:size(Pg, 2)`
120	`clusterCenter = mean(cat(1, Pg{:,k}), 1);`	128	`clusterCenter = mean(cat(1, Pg{:,k}), 1);`
121	`if(sum((pg - clusterCenter).^2) < 3^2)`	129	`if(sum((pg - clusterCenter).^2) < 3^2)`
122	`% store in global frame`	130	`% store in global frame`
123	`Pg{i,k} = pg;`	131	`Pg{i,k} = pg;`
124	`% store in local frame`	132	`% store in local frame`
125	`P{i,k} = p;`	133	`P{i,k} = p;`
126	`matched = true;`	134	`matched = true;`
127	`break;`	135	`break;`
128	`end`	136	`end`
129	`end`	137	`end`
130	`% create new cluster`	138	`% create new cluster`
131	`if(not(matched))`	139	`if(not(matched))`
132	`Pg{i,end+1} = pg;`	140	`Pg{i,end+1} = pg;`
133	`P{i,end+1} = p;`	141	`P{i,end+1} = p;`
134	`end`	142	`end`
135	`end`	143	`end`
136	`end`	144	`end`
137		145
138	`% run optimization`	146	`% run optimization`
139	`alignment = groupwiseOrthogonalProcrustes(P, initialAlign);`	147	`alignment = groupwiseOrthogonalProcrustes(P, initialAlign);`
140		148
141	`% show found markers in optimized alignment`	149	`% show found markers in optimized alignment`
142	`figure;`	150	`figure;`
143	`hold('on');`	151	`hold('on');`
144	`for i=1:nSubScans`	152	`for i=1:nSubScans`
145	`Ri = alignment(i).Rotation;`	153	`Ri = alignment(i).Rotation;`
146	`Ti = alignment(i).Translation;`	154	`Ti = alignment(i).Translation;`
147		155
148	`Ea = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);`	156	`Ea = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);`
149		157
150	`% bring point cloud into optimized alignment`	158	`% bring point cloud into optimized alignment`
151	`pc = pcread(initialAlign(i).FileName);`	159	`pc = pcread(initialAlign(i).FileName);`
152	`tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);`	160	`tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);`
153	`pcg = pctransform(pc, tform);`	161	`pcg = pctransform(pc, tform);`
154		162
155	`pcshow(pcg);`	163	`pcshow(pcg);`
156	`xlabel('x');`	164	`xlabel('x');`
157	`ylabel('y');`	165	`ylabel('y');`
158	`zlabel('z');`	166	`zlabel('z');`
159		167
160	`plot3(Ea(:,1), Ea(:,2), Ea(:,3), '.', 'MarkerSize', 15);`	168	`plot3(Ea(:,1), Ea(:,2), Ea(:,3), '.', 'MarkerSize', 15);`
161	`title('Optimized Alignment');`	169	`title('Optimized Alignment');`
162	`end`	170	`end`
163		171
164	`% write to ALN file`	172	`% write to ALN file`
165	`for i=1:length(alignment)`	173	`for i=1:length(alignment)`
166	`alignment(i).FileName = initialAlign(i).FileName;`	174	`alignment(i).FileName = initialAlign(i).FileName;`
167	`end`	175	`end`
168		176
169	`writeMeshLabALN(alignment, strrep(alnFileName, '.aln', 'Optimized.aln'));`	177	`writeMeshLabALN(alignment, strrep(alnFileName, '.aln', 'Optimized.aln'));`
170		178
171	`end`	179	`end`
172		180
173	`function e = autoDetectMarkers(frame, P, pointCloud)`	181	`function e = autoDetectMarkers(frame, P, pointCloud)`
174		182
175	`% create mask based on morphology`	183	`% create mask based on morphology`
176	`bw = imbinarize(rgb2gray(frame));`	184	`bw = imbinarize(rgb2gray(frame));`
177	`cc = bwconncomp(bw);`	185	`cc = bwconncomp(bw);`
178	`rp = regionprops(cc, 'Area', 'Solidity', 'Eccentricity', 'Centroid');`	186	`rp = regionprops(cc, 'Area', 'Solidity', 'Eccentricity', 'Centroid');`
179	`idx = ([rp.Area] > 100 & [rp.Area] < 1000 & [rp.Solidity] > 0.9);`	187	`idx = ([rp.Area] > 100 & [rp.Area] < 1000 & [rp.Solidity] > 0.9);`
180		188
181	`initialGuesses = cat(1, rp(idx).Centroid);`	189	`initialGuesses = cat(1, rp(idx).Centroid);`
182		190
183	`[e, ~] = detectMarkersSubpix(frame, initialGuesses, P, pointCloud);`	191	`[e, ~] = detectMarkersSubpix(frame, initialGuesses, P, pointCloud);`
184		192
185	`figure;`	193	`figure;`
186	`imshow(frame);`	194	`imshow(frame);`
187	`hold('on');`	195	`hold('on');`
188	`plot(e(:,1), e(:,2), 'rx', 'MarkerSize', 15);`	196	`plot(e(:,1), e(:,2), 'rx', 'MarkerSize', 15);`
189	`drawnow;`	197	`drawnow;`
190	`end`	198	`end`
191		199
192	`function e = manuallyDetectMarkers(frame, P, pointCloud)`	200	`function e = manuallyDetectMarkers(frame, P, pointCloud)`
193		201
194	`e = [];`	202	`e = [];`
195	`%edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);`	203	`%edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);`
196		204
197	`figure;`	205	`figure;`
198	`hold('on');`	206	`hold('on');`
199	`imshow(frame);`	207	`imshow(frame);`
200	`title('Close figure to end.');`	208	`title('Close figure to end.');`
201	`set(gcf, 'pointer', 'crosshair');`	209	`set(gcf, 'pointer', 'crosshair');`
202	`set(gcf, 'WindowButtonDownFcn', @clickCallback);`	210	`set(gcf, 'WindowButtonDownFcn', @clickCallback);`
203		211
204	`uiwait;`	212	`uiwait;`
205		213
206	`function clickCallback(caller, ~)`	214	`function clickCallback(caller, ~)`
207		215
208	`p = get(gca, 'CurrentPoint');`	216	`p = get(gca, 'CurrentPoint');`
209	`p = p(1, 1:2);`	217	`p = p(1, 1:2);`
210		218
211	`[el, ~] = detectMarkersSubpix(frame, p, P, pointCloud);`	219	`[el, ~] = detectMarkersSubpix(frame, p, P, pointCloud);`
212	`e = [e; el(:, 1:2)];`	220	`e = [e; el(:, 1:2)];`
213		221
214	`if(not(isempty(el)))`	222	`if(not(isempty(el)))`
215	`figure(caller);`	223	`figure(caller);`
216	`hold('on');`	224	`hold('on');`
217	`plot(el(1), el(2), 'rx', 'MarkerSize', 15);`	225	`plot(el(1), el(2), 'rx', 'MarkerSize', 15);`
218	`end`	226	`end`
219	`end`	227	`end`
220		228
221	`end`	229	`end`
222		230
223	`function [e, conf] = detectMarkersSubpix(frame, initGuesses, P, Q)`	231	`function [e, conf] = detectMarkersSubpix(frame, initGuesses, P, Q)`
224		232
225	`% create mask based on morphology`	233	`% create mask based on morphology`
226	`bw = imbinarize(rgb2gray(frame));`	234	`bw = imbinarize(rgb2gray(frame));`
227	`cc = bwconncomp(bw);`	235	`cc = bwconncomp(bw);`
228	`labels = labelmatrix(cc);`	236	`labels = labelmatrix(cc);`
229		237
230	`% project point cloud into image`	238	`% project point cloud into image`
231	`q = [Q ones(size(Q,1),1)]*P;`	239	`q = [Q ones(size(Q,1),1)]*P;`
232	`q = q./[q(:,3) q(:,3) q(:,3)];`	240	`q = q./[q(:,3) q(:,3) q(:,3)];`
233		241
234	`e = zeros(size(initGuesses));`	242	`e = zeros(size(initGuesses));`
235	`conf = zeros(size(initGuesses, 1), 1);`	243	`conf = zeros(size(initGuesses, 1), 1);`
236		244
237	`nMarkersFound = 0;`	245	`nMarkersFound = 0;`
238		246
239	`for i=1:size(initGuesses, 1)`	247	`for i=1:size(initGuesses, 1)`
240		248
241	`labelId = labels(round(initGuesses(i,2)), round(initGuesses(i,1)));`	249	`labelId = labels(round(initGuesses(i,2)), round(initGuesses(i,1)));`
242	`labelMask = (labels == labelId);`	250	`labelMask = (labels == labelId);`
243	`labelMask = imdilate(labelMask, strel('disk', 3, 0));`	251	`labelMask = imdilate(labelMask, strel('disk', 3, 0));`
244		252
245	`if(sum(sum(labelMask)) < 10 \|\| sum(sum(labelMask)) > 1000)`	253	`if(sum(sum(labelMask)) < 10 \|\| sum(sum(labelMask)) > 1000)`
246	`continue;`	254	`continue;`
247	`end`	255	`end`
248		256
249	`% determine 3D points that are part of the marker`	257	`% determine 3D points that are part of the marker`
250	`% note: we should probably undistort labelMask`	258	`% note: we should probably undistort labelMask`
251	`pointMask = false(size(q, 1), 1);`	259	`pointMask = false(size(q, 1), 1);`
252	`for j=1:size(q,1)`	260	`for j=1:size(q,1)`
253	`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)`	261	`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)`
254	`continue;`	262	`continue;`
255	`end`	263	`end`
256		264
257	`if(labelMask(round(q(j,2)), round(q(j,1))))`	265	`if(labelMask(round(q(j,2)), round(q(j,1))))`
258	`pointMask(j) = true;`	266	`pointMask(j) = true;`
259	`end`	267	`end`
260	`end`	268	`end`
261		269
262	`if(sum(pointMask)) < 10`	270	`if(sum(pointMask)) < 10`
263	`continue;`	271	`continue;`
264	`end`	272	`end`
265		273
266	`% project 3D points onto local plane`	274	`% project 3D points onto local plane`
267	`[~, sc, ~] = pca(Q(pointMask, :));`	275	`[~, sc, ~] = pca(Q(pointMask, :));`
268	`Qlocal = sc(:, 1:2);`	276	`Qlocal = sc(:, 1:2);`
269		277
270	`% synthetic marker in high res. space`	278	`% synthetic marker in high res. space`
271	`m = zeros(151, 151);`	279	`m = zeros(151, 151);`
272	`[x, y] = meshgrid(1:151, 1:151);`	280	`[x, y] = meshgrid(1:151, 1:151);`
273	`m((x(:)-76).^2 + (y(:)-76).^2 <= 50^2) = 1.0;`	281	`m((x(:)-76).^2 + (y(:)-76).^2 <= 50^2) = 1.0;`
274		282
275	`% relation between marker space (px) and true marker/local plane(mm)`	283	`% relation between marker space (px) and true marker/local plane(mm)`
276	`% true marker diameter is 1.75mm`	284	`% true marker diameter is 1.75mm`
277	`mScale = 101/1.8; %px/mm`	285	`mScale = 101/1.8; %px/mm`
278	`mShift = 76; %px`	286	`mShift = 76; %px`
279		287
280	`% build homography from image to marker space`	288	`% build homography from image to marker space`
281	`H = fitgeotrans(q(pointMask, 1:2), mScale*Qlocal+mShift, 'projective');`	289	`H = fitgeotrans(q(pointMask, 1:2), mScale*Qlocal+mShift, 'projective');`
282		290
283	`% bring image of marker into marker space`	291	`% bring image of marker into marker space`
284	`imMarkerSpace = imwarp(frame, H, 'OutputView', imref2d(size(m)));`	292	`imMarkerSpace = imwarp(frame, H, 'OutputView', imref2d(size(m)));`
285	`imMarkerSpace = rgb2gray(im2double(imMarkerSpace));`	293	`imMarkerSpace = rgb2gray(im2double(imMarkerSpace));`
286		294
287	`%figure; imshowpair(imMarkerSpace, m);`	295	`%figure; imshowpair(imMarkerSpace, m);`
288		296
289	`% perform image registration`	297	`% perform image registration`
290	`% might be better off using subpixel image correlation`	298	`% might be better off using subpixel image correlation`
291	`[opt, met] = imregconfig('multimodal');`	299	`[opt, met] = imregconfig('multimodal');`
292	`T = imregtform(m, imMarkerSpace, 'translation', opt, met, 'DisplayOptimization', true);`	300	`T = imregtform(m, imMarkerSpace, 'translation', opt, met, 'DisplayOptimization', true);`
293		301
294	`rege = imwarp(m, T, 'OutputView', imref2d(size(m)));`	302	`rege = imwarp(m, T, 'OutputView', imref2d(size(m)));`
295	`%figure; imshowpair(imMarkerSpace, rege);`	303	`%figure; imshowpair(imMarkerSpace, rege);`
296		304
297	`% measure of correlation`	305	`% measure of correlation`
298	`confI = sum(sum(imMarkerSpace .* rege))/sqrt(sum(sum(imMarkerSpace) * sum(sum(rege))));`	306	`confI = sum(sum(imMarkerSpace .* rege))/sqrt(sum(sum(imMarkerSpace) * sum(sum(rege))));`
299		307
300	`if confI<0.4`	308	`if confI<0.4`
301	`continue;`	309	`continue;`
302	`end`	310	`end`
303		311
304	`fprintf('Found marker with confidence: %f\n', confI);`	312	`fprintf('Found marker with confidence: %f\n', confI);`
305		313
306	`% transform marker space coordinates (76,76) to frame space`	314	`% transform marker space coordinates (76,76) to frame space`
307	`el = T.transformPointsForward([76, 76]);`	315	`el = T.transformPointsForward([76, 76]);`
308	`el = H.transformPointsInverse(el);`	316	`el = H.transformPointsInverse(el);`
309		317
310	`nMarkersFound = nMarkersFound+1;`	318	`nMarkersFound = nMarkersFound+1;`
311	`e(nMarkersFound,:) = el;`	319	`e(nMarkersFound,:) = el;`
312	`conf(nMarkersFound) = confI;`	320	`conf(nMarkersFound) = confI;`
313	`end`	321	`end`
314		322
315	`e = e(1:nMarkersFound, :);`	323	`e = e(1:nMarkersFound, :);`
316	`conf = conf(1:nMarkersFound);`	324	`conf = conf(1:nMarkersFound);`
317	`end`	325	`end`
318		326
319	`function E = projectOntoPointCloud(e, P, pointCloud)`	327	`function E = projectOntoPointCloud(e, P, pointCloud)`
320		328
321	`q = [pointCloud ones(size(pointCloud,1),1)]*P;`	329	`q = [pointCloud ones(size(pointCloud,1),1)]*P;`
322	`q = q(:,1:2)./[q(:,3) q(:,3)];`	330	`q = q(:,1:2)./[q(:,3) q(:,3)];`
323		331
324	`E = nan(size(e,1), 3);`	332	`E = nan(size(e,1), 3);`
325		333
326	`for i=1:size(e, 1)`	334	`for i=1:size(e, 1)`
327	`sqDists = sum((q - repmat(e(i,:), size(q, 1), 1)).^2, 2);`	335	`sqDists = sum((q - repmat(e(i,:), size(q, 1), 1)).^2, 2);`
328		336
329	`[minSqDist, minSqDistIdx] = min(sqDists);`	337	`[sqDistsSorted, sortIdx] = sort(sqDists);`
330		338
331	`if(minSqDist < 2^2)`	339	`neighbors = (sqDistsSorted < 4.0^2);`
332		340
-		341	`distsSorted = sqrt(sqDistsSorted(neighbors));`
-		342	`invDistsSorted = 1.0/distsSorted;`
-		343	`sortIdx = sortIdx(neighbors);`
-		344
-		345	`nNeighbors = sum(neighbors);`
-		346
-		347	`if(nNeighbors >= 2)`
333	`E(i, :) = pointCloud(minSqDistIdx, :);`	348	`E(i, :) = 0;`
-		349	`for j=1:nNeighbors`
-		350	`E(i, :) = E(i, :) + invDistsSorted(j)/sum(invDistsSorted) * pointCloud(sortIdx(j), :);`
334		351	`end`
335	`end`	352	`end`
336		353
337	`end`	354	`end`
338	`end`	355	`end`
339		356
340		357

Subversion Repositories seema-scanner

(root)/matlab/alignSubScansMarkers.m/ – Rev 215 → 216