Subversion Repositories seema-scanner

Rev

Rev 211 | Rev 213 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 211 Rev 212
Line 1... Line 1...
1 
function MSE = alignSubScansMarkers(calibrationFileName, scanDir, alnFileName)
 1 
function MSE = alignSubScansMarkers(calibrationFileName, scanDir, 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 the specified 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 
 
Line 46... Line 46...
46
46
47 
    % load white frames
 47 
    % load white frames
48 
    frame0 = imread(fullfile(scanDir, ['sequence_' idString], 'frames0_0.png'));
 48 
    frame0 = imread(fullfile(scanDir, ['sequence_' idString], 'frames0_0.png'));
49 
    frame1 = imread(fullfile(scanDir, ['sequence_' idString], 'frames1_0.png'));
 49 
    frame1 = imread(fullfile(scanDir, ['sequence_' idString], 'frames1_0.png'));
50 
 
 50 
 
51 
%     e0Coords = autoDetectEllipses(frame0, ep);
 51 
    %e0Coords = autoDetectMarkers(frame0, ep);
52 
%     e1Coords = autoDetectEllipses(frame1, ep);
 52 
    %e1Coords = autoDetectMarkers(frame1, ep);
53
53
54 
    e0Coords = manuallyDetectEllipses(frame0, ep);
 54 
    e0Coords = manuallyDetectMarkers(frame0, ep, P0, Q);
55 
    e1Coords = manuallyDetectEllipses(frame1, ep);
 55 
    e1Coords = manuallyDetectMarkers(frame1, ep, P1, Q);
56
56
57 
    e0Coords = undistortPoints(e0Coords, camParams0);
 57 
    e0Coords = undistortPoints(e0Coords, camParams0);
58 
    e1Coords = undistortPoints(e1Coords, camParams1);
 58 
    e1Coords = undistortPoints(e1Coords, camParams1);
59 
 
 59 
 
60 
    % match ellipse candidates between cameras based on projection
 60 
    % match ellipse candidates between cameras based on projection
Line 86... Line 86...
86 
    Ti = initialAlign(i).Translation;
 86 
    Ti = initialAlign(i).Translation;
87
87
88 
    Eg{i} = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);
 88 
    Eg{i} = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);
89 
end
 89 
end
90 
 
 90 
 
91 
% match markers between poses using initial alignment
 91 
% show found markers in initial alignment
92 
 
 92 
figure;
- 
 
 93 
hold('on');
93 
for i=1:nSubScans
 94 
for i=1:nSubScans
94 
    % fix Ri to be orthogonal
 95 
    % fix Ri to be orthogonal
95 
    [U,~,V] = svd(initialAlign(i).Rotation);
 96 
    [U,~,V] = svd(initialAlign(i).Rotation);
96 
    Ri = U*V';
 97 
    Ri = U*V';
97
98
98 
    % bring point cloud into initial alignment
 99 
    % bring point cloud into initial alignment
99 
    pc = pcread(fullfile(scanDir, initialAlign(i).FileName));
 100 
    pc = pcread(fullfile(scanDir, initialAlign(i).FileName));
100 
    tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);
 101 
    tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);
101 
    pcg = pctransform(pc, tform);
 102 
    pcg = pctransform(pc, tform);
102
103
103 
    figure;
 - 
 
104 
    hold('on');
 - 
 
105 
	plot3(Eg{i}(:,1), Eg{i}(:,2), Eg{i}(:,3), 'r.', 'MarkerSize', 15);
 - 
 
106 
    pcshow(pcg);
 104 
    pcshow(pcg);
107 
    xlabel('x');
 105 
    xlabel('x');
108 
    ylabel('y');
 106 
    ylabel('y');
109 
    zlabel('z');
 107 
    zlabel('z');
- 
 
 108
- 
 
 109 
    plot3(Eg{i}(:,1), Eg{i}(:,2), Eg{i}(:,3), '.', 'MarkerSize', 15);
- 
 
 110 
end
- 
 
 111 
 
- 
 
 112 
% match markers between poses using initial alignment
- 
 
 113 
Pg = {};
- 
 
 114 
P = {};
- 
 
 115 
for i=1:nSubScans
- 
 
 116 
    for j=1:size(Eg{i}, 1)
- 
 
 117 
        pg = Eg{i}(j,:);
- 
 
 118 
        p = E{i}(j,:);
- 
 
 119 
        matched = false;
- 
 
 120 
        for k=1:size(Pg, 2)
- 
 
 121 
            clusterCenter = mean(cat(1, Pg{:,k}), 1);
- 
 
 122 
            if(sum((pg - clusterCenter).^2) < 3^2)
- 
 
 123 
                % store in global frame
- 
 
 124 
                Pg{i,k} = pg;
- 
 
 125 
                % store in local frame
- 
 
 126 
                P{i,k} = p;
- 
 
 127 
                matched = true;
- 
 
 128 
                break;
- 
 
 129 
            end
- 
 
 130 
        end
- 
 
 131 
        % create new cluster
- 
 
 132 
        if(not(matched))
- 
 
 133 
            Pg{i,end+1} = pg;
- 
 
 134 
            P{i,end+1} = p;
- 
 
 135 
        end
- 
 
 136 
    end
110 
end
 137 
end
111 
 
 138 
 
- 
 
 139 
% run optimization
- 
 
 140 
alignment = groupwiseOrthogonalProcrustes(P, initialAlign);
112 
 
 141 
 
- 
 
 142 
for i=1:length(alignment)
- 
 
 143 
    alignment(i).FileName = initialAlign(i).FileName;
113 
end
 144 
end
114 
 
 145 
 
- 
 
 146 
writeMeshLabALN(alignment, strrep(alnFileName, '.aln', '_opt.aln'));
- 
 
 147 
 
- 
 
 148 
end
- 
 
 149 
 
115 
function e = autoDetectEllipses(frame, ep)
 150 
function e = autoDetectMarkers(frame, ep)
116 
 
 151 
 
117 
    % create mask based on morphology
 152 
    % create mask based on morphology
118 
    bw = imbinarize(rgb2gray(frame));
 153 
    bw = imbinarize(rgb2gray(frame));
119 
    cc = bwconncomp(bw);
 154 
    cc = bwconncomp(bw);
120 
    rp = regionprops(cc, 'Area', 'Solidity', 'Eccentricity', 'Centroid');
 155 
    rp = regionprops(cc, 'Area', 'Solidity', 'Eccentricity', 'Centroid');
121 
    idx = find([rp.Area] > 100 & [rp.Area] < 1000 & [rp.Solidity] > 0.9);
 156 
    idx = find([rp.Area] > 100 & [rp.Area] < 1000 & [rp.Solidity] > 0.9);
122 
    mask = ismember(labelmatrix(cc0), idx);
 157 
    mask = ismember(labelmatrix(cc), idx);
123 
    mask = imdilate(mask, strel('disk', 20, 0));
 158 
    mask = imdilate(mask, strel('disk', 20, 0));
124
159
125 
    % detect ellipses within mask
 160 
    % detect ellipses within mask
126 
    edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);
 161 
    edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);
127 
    edges(~mask) = 0;
 162 
    edges(~mask) = 0;
128 
    ep.numBest = 10;
163 
    ep.numBest = 10;
129 
    e = ellipseDetection(edges, ep);
 164 
    el = ellipseDetection(edges, ep);
- 
 
 165
- 
 
 166 
    e = el(:, 1:2);
130 
end
 167 
end
131 
 
 168 
 
132 
function e = manuallyDetectEllipses(frame, ep)
 169 
function e = manuallyDetectMarkers(frame, ep, P, pointCloud)
133
170
- 
 
 171 
    e = [];
- 
 
 172 
	edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);
- 
 
 173 
 
134 
    figure;
174 
    figure;
- 
 
 175 
    hold('on');
135 
    imshow(frame);
 176 
    imshow(frame);
136 
    title('Press return to end');
 177 
    title('Close figure to end.');
137 
 
 - 
 
138 
    p = ginput();
 - 
 
139 
 
 - 
 
140 
    e = [];
 - 
 
141 
    for i=1:size(p, 1)
 - 
 
142 
        % create mask around selected point
 178 
    set(gcf, 'pointer', 'crosshair');
143 
        mask = false(size(frame, 1), size(frame, 2));
 - 
 
144 
 
 - 
 
145 
        mask(round(p(i,2)), round(p(i,1))) = true;
 - 
 
146 
        mask = imdilate(mask, strel('disk', 100, 0));
 179 
    set(gcf, 'WindowButtonDownFcn', @clickCallback);
147 
 
 180
148 
        % detect ellipses within mask
 - 
 
149 
        edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);
 - 
 
150 
        edges(~mask) = 0;
 181 
    uiwait;
151 
 
 182 
 
- 
 
 183 
    function clickCallback(~, ~)
- 
 
 184
- 
 
 185 
        p = get(gca, 'CurrentPoint');
- 
 
 186 
        p = p(1, 1:2);
- 
 
 187
- 
 
 188 
%         % create mask around selected point
- 
 
 189 
%         mask = false(size(frame, 1), size(frame, 2));
- 
 
 190 
%
- 
 
 191 
%         mask(round(p(2)), round(p(1))) = true;
- 
 
 192 
%         mask = imdilate(mask, strel('disk', 100, 0));
- 
 
 193 
%
- 
 
 194 
%         % detect ellipses within mask
- 
 
 195 
%         edgesI = edges;
- 
 
 196 
%         edgesI(~mask) = 0;
- 
 
 197 
%
152 
        ep.numBest = 1;
198 
%         ep.numBest = 1;
153 
        el = ellipseDetection(edges, ep);
 199 
%         el = ellipseDetection(edgesI, ep);
- 
 
 200 
%
- 
 
 201 
%         ellipse(el(:,3), el(:,4), el(:,5)*pi/180, el(:,1), el(:,2), 'r');
- 
 
 202 
%
- 
 
 203 
%         e = [e; el(:, 1:2)];
154 
 
 204 
 
- 
 
 205 
        [el, conf] = detectMarkersSubpix(frame, p, P, pointCloud)
155 
        e = [e; el(:, 1:2)];
 206 
        e = [e; el(:, 1:2)];
156 
    end
 207 
    end
157
208
158 
    close(gcf);
 - 
 
159
- 
 
160 
end
 209 
end
161 
 
 210 
 
162 
function E = projectOntoPointCloud(e, P, Q)
 211 
function [e, conf] = detectMarkersSubpix(frame, initGuesses, P, Q)
- 
 
 212 
 
- 
 
 213 
    % create mask based on morphology
- 
 
 214 
    bw = imbinarize(rgb2gray(frame));
- 
 
 215 
    cc = bwconncomp(bw);
- 
 
 216 
    labels = labelmatrix(cc);
163 
 
 217 
 
- 
 
 218 
    % project point cloud into image
164 
    q = [Q ones(size(Q,1),1)]*P;
 219 
    q = [Q ones(size(Q,1),1)]*P;
- 
 
 220 
    q = q./[q(:,3) q(:,3) q(:,3)];
- 
 
 221
- 
 
 222 
    for i=1:size(initGuesses, 1)
- 
 
 223
- 
 
 224 
        labelId = labels(round(initGuesses(i,2)), round(initGuesses(i,1)));
- 
 
 225 
        labelMask = (labels == labelId);
- 
 
 226 
        labelMask = imdilate(labelMask, strel('disk', 3, 0));
- 
 
 227
- 
 
 228 
        % determine 3D points that are part of the marker
- 
 
 229 
        pointMask = false(size(q, 1), 1);
- 
 
 230 
        for j=1:size(q,1)
- 
 
 231 
            if(labelMask(round(q(j,2)), round(q(j,1))))
- 
 
 232 
                pointMask(j) = true;
- 
 
 233 
            end
- 
 
 234 
        end
- 
 
 235
- 
 
 236 
        % build homography
- 
 
 237 
        H = fitgeotrans(Q(pointMask, :), q(pointMask, :), 'projective');
- 
 
 238
- 
 
 239 
    end
- 
 
 240
- 
 
 241 
    e = initGuesses;
- 
 
 242 
    conf = 1.0;
- 
 
 243 
 
- 
 
 244 
end
- 
 
 245 
 
- 
 
 246 
function E = projectOntoPointCloud(e, P, pointCloud)
- 
 
 247 
 
- 
 
 248 
    q = [pointCloud ones(size(pointCloud,1),1)]*P;
165 
    q = q(:,1:2)./[q(:,3) q(:,3)];
 249 
    q = q(:,1:2)./[q(:,3) q(:,3)];
166 
 
 250 
 
167 
    E = nan(size(e,1), 3);
 251 
    E = nan(size(e,1), 3);
168
252
169 
    for i=1:size(e, 1)
 253 
    for i=1:size(e, 1)
Line 171... Line 255...
171
255
172 
        [minSqDist, minSqDistIdx] = min(sqDists);
 256 
        [minSqDist, minSqDistIdx] = min(sqDists);
173
257
174 
        if(minSqDist < 2^2)
 258 
        if(minSqDist < 2^2)
175
259
176 
            E(i, :) = Q(minSqDistIdx, :);
 260 
            E(i, :) = pointCloud(minSqDistIdx, :);
177
261
178 
        end
 262 
        end
179
263
180 
    end
264 
    end
181 
end
 265 
end