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);
 
% 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 < 5^2)
            nMatchedPairs = nMatchedPairs + 1;
            matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];
        end
    end
    matchedPairs = matchedPairs(1:nMatchedPairs, :);
    
    % triangulate marker centers (lens correction has been performed)
    [E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);
    E{i} = E{i}(e<0.5, :);
    
    % bring into initial alignment
    [U,~,V] = svd(initialAlign(i).Rotation);
    Ri = U*V';
    Ti = initialAlign(i).Translation;
    
    Eg{i} = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);
end
 
% 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(fullfile(scanDir, 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(fullfile(scanDir, 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(labelMask(round(q(j,2)), round(q(j,1))))
                pointMask(j) = true;
            end
        end
        
        if(sum(pointMask)) < 50
            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);
        
        [minSqDist, minSqDistIdx] = min(sqDists);
        
        if(minSqDist < 2^2)
           
            E(i, :) = pointCloud(minSqDistIdx, :);
            
        end
            
    end    
end
 
 

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

Subversion Repositories seema-scanner

(root)/matlab/alignSubScansMarkers.m – Rev 213 → 214