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(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);
        
        [minSqDist, minSqDistIdx] = min(sqDists);
        
        if(minSqDist < 2^2)
           
            E(i, :) = pointCloud(minSqDistIdx, :);
            
        end
            
    end    
end
 
 

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

Subversion Repositories seema-scanner

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