Subversion Repositories seema-scanner

Rev

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

Rev 212 Rev 213
Line 25... Line 25...
25 
% matlab struct for triangulation
 25 
% matlab struct for triangulation
26 
camStereoParams = stereoParameters(camParams0, camParams1, calibration.R1', calibration.T1');
 26 
camStereoParams = stereoParameters(camParams0, camParams1, calibration.R1', calibration.T1');
27 
 
 27 
 
28 
nSubScans = length(whiteFrameDirs);
 28 
nSubScans = length(whiteFrameDirs);
29 
 
 29 
 
30 
% ellipse detection settings
 - 
 
31 
ep = struct('minMajorAxis', 25, 'maxMajorAxis', 30, 'minAspectRatio', 0.4, 'randomize', 0, 'smoothStddev', 2);
 - 
 
32 
 
 - 
 
33 
% 3D coordinates of markers in local camera frame
 30 
% 3D coordinates of markers in local camera frame
34 
E = cell(nSubScans, 1);
 31 
E = cell(nSubScans, 1);
35 
 
 32 
 
36 
% 3D coordinates of markers in global initial alignment
 33 
% 3D coordinates of markers in global initial alignment
37 
Eg = cell(size(E));
 34 
Eg = cell(size(E));
Line 46... Line 43...
46
43
47 
    % load white frames
 44 
    % load white frames
48 
    frame0 = imread(fullfile(scanDir, ['sequence_' idString], 'frames0_0.png'));
 45 
    frame0 = imread(fullfile(scanDir, ['sequence_' idString], 'frames0_0.png'));
49 
    frame1 = imread(fullfile(scanDir, ['sequence_' idString], 'frames1_0.png'));
 46 
    frame1 = imread(fullfile(scanDir, ['sequence_' idString], 'frames1_0.png'));
50 
 
 47 
 
51 
    %e0Coords = autoDetectMarkers(frame0, ep);
 48 
    e0Coords = autoDetectMarkers(frame0, P0, Q);
52 
    %e1Coords = autoDetectMarkers(frame1, ep);
 49 
    e1Coords = autoDetectMarkers(frame1, P1, Q);
53
50
54 
    e0Coords = manuallyDetectMarkers(frame0, ep, P0, Q);
 51 
    %e0Coords = manuallyDetectMarkers(frame0, P0, Q);
55 
    e1Coords = manuallyDetectMarkers(frame1, ep, P1, Q);
 52 
    %e1Coords = manuallyDetectMarkers(frame1, P1, Q);
56
53
57 
    e0Coords = undistortPoints(e0Coords, camParams0);
 54 
    e0Coords = undistortPoints(e0Coords, camParams0);
58 
    e1Coords = undistortPoints(e1Coords, camParams1);
 55 
    e1Coords = undistortPoints(e1Coords, camParams1);
59 
 
 56 
 
60 
    % match ellipse candidates between cameras based on projection
 57 
    % match ellipse candidates between cameras based on projection
Line 105... Line 102...
105 
    xlabel('x');
 102 
    xlabel('x');
106 
    ylabel('y');
 103 
    ylabel('y');
107 
    zlabel('z');
 104 
    zlabel('z');
108
105
109 
    plot3(Eg{i}(:,1), Eg{i}(:,2), Eg{i}(:,3), '.', 'MarkerSize', 15);
 106 
    plot3(Eg{i}(:,1), Eg{i}(:,2), Eg{i}(:,3), '.', 'MarkerSize', 15);
- 
 
 107 
    title('Initial Alignment');
110 
end
 108 
end
111 
 
 109 
 
112 
% match markers between poses using initial alignment
 110 
% match markers between poses using initial alignment
113 
Pg = {};
 111 
Pg = {};
114 
P = {};
 112 
P = {};
Line 137... Line 135...
137 
end
 135 
end
138 
 
 136 
 
139 
% run optimization
 137 
% run optimization
140 
alignment = groupwiseOrthogonalProcrustes(P, initialAlign);
 138 
alignment = groupwiseOrthogonalProcrustes(P, initialAlign);
141 
 
 139 
 
- 
 
 140 
% show found markers in optimized alignment
- 
 
 141 
figure;
- 
 
 142 
hold('on');
- 
 
 143 
for i=1:nSubScans
- 
 
 144 
    Ri = alignment(i).Rotation;
- 
 
 145 
    Ti = alignment(i).Translation;
- 
 
 146
- 
 
 147 
    Ea = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);
- 
 
 148
- 
 
 149 
    % bring point cloud into optimized alignment
- 
 
 150 
    pc = pcread(fullfile(scanDir, initialAlign(i).FileName));
- 
 
 151 
    tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);
- 
 
 152 
    pcg = pctransform(pc, tform);
- 
 
 153
- 
 
 154 
    pcshow(pcg);
- 
 
 155 
    xlabel('x');
- 
 
 156 
    ylabel('y');
- 
 
 157 
    zlabel('z');
- 
 
 158
- 
 
 159 
    plot3(Ea(:,1), Ea(:,2), Ea(:,3), '.', 'MarkerSize', 15);
- 
 
 160 
    title('Optimized Alignment');
- 
 
 161 
end
- 
 
 162 
 
- 
 
 163 
% write to ALN file
142 
for i=1:length(alignment)
 164 
for i=1:length(alignment)
143 
    alignment(i).FileName = initialAlign(i).FileName;
 165 
    alignment(i).FileName = initialAlign(i).FileName;
144 
end
 166 
end
145 
 
 167 
 
146 
writeMeshLabALN(alignment, strrep(alnFileName, '.aln', '_opt.aln'));
 168 
writeMeshLabALN(alignment, strrep(alnFileName, '.aln', '_opt.aln'));
147 
 
 169 
 
148 
end
 170 
end
149 
 
 171 
 
150 
function e = autoDetectMarkers(frame, ep)
 172 
function e = autoDetectMarkers(frame, P, pointCloud)
151 
 
 173 
 
152 
    % create mask based on morphology
 174 
    % create mask based on morphology
153 
    bw = imbinarize(rgb2gray(frame));
 175 
    bw = imbinarize(rgb2gray(frame));
154 
    cc = bwconncomp(bw);
 176 
    cc = bwconncomp(bw);
155 
    rp = regionprops(cc, 'Area', 'Solidity', 'Eccentricity', 'Centroid');
 177 
    rp = regionprops(cc, 'Area', 'Solidity', 'Eccentricity', 'Centroid');
156 
    idx = find([rp.Area] > 100 & [rp.Area] < 1000 & [rp.Solidity] > 0.9);
 178 
    idx = ([rp.Area] > 100 & [rp.Area] < 1000 & [rp.Solidity] > 0.9);
157 
    mask = ismember(labelmatrix(cc), idx);
 - 
 
158 
    mask = imdilate(mask, strel('disk', 20, 0));
 - 
 
159
179
160 
    % detect ellipses within mask
 180 
    initialGuesses = cat(1, rp(idx).Centroid);
- 
 
 181 
 
161 
    edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);
 182 
    [e, ~] = detectMarkersSubpix(frame, initialGuesses, P, pointCloud);
- 
 
 183 
 
- 
 
 184 
    figure;
162 
    edges(~mask) = 0;
 185 
    imshow(frame);
163 
    ep.numBest = 10;
186 
        hold('on');
164 
    el = ellipseDetection(edges, ep);
 187 
    plot(e(:,1), e(:,2), 'rx', 'MarkerSize', 15);
165
- 
 
166 
    e = el(:, 1:2);
 188 
    drawnow;
167 
end
 189 
end
168 
 
 190 
 
169 
function e = manuallyDetectMarkers(frame, ep, P, pointCloud)
 191 
function e = manuallyDetectMarkers(frame, P, pointCloud)
170
192
171 
    e = [];
 193 
    e = [];
172 
	edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);
 194 
	%edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);
173 
 
 195 
 
174 
    figure;
196 
    figure;
175 
    hold('on');
 197 
    hold('on');
176 
    imshow(frame);
 198 
    imshow(frame);
177 
    title('Close figure to end.');
 199 
    title('Close figure to end.');
178 
    set(gcf, 'pointer', 'crosshair');
200 
    set(gcf, 'pointer', 'crosshair');
179 
    set(gcf, 'WindowButtonDownFcn', @clickCallback);
 201 
    set(gcf, 'WindowButtonDownFcn', @clickCallback);
180
202
181 
    uiwait;
 203 
    uiwait;
182 
 
 204 
 
183 
    function clickCallback(~, ~)
 205 
    function clickCallback(caller, ~)
184
206
185 
        p = get(gca, 'CurrentPoint');
207 
        p = get(gca, 'CurrentPoint');
186 
        p = p(1, 1:2);
 208 
        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 
%
- 
 
198 
%         ep.numBest = 1;
- 
 
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)];
 - 
 
204 
 
 209 
 
205 
        [el, conf] = detectMarkersSubpix(frame, p, P, pointCloud)
 210 
        [el, ~] = detectMarkersSubpix(frame, p, P, pointCloud);
206 
        e = [e; el(:, 1:2)];
 211 
        e = [e; el(:, 1:2)];
- 
 
 212
- 
 
 213 
        if(not(isempty(el)))
- 
 
 214 
            figure(caller);
- 
 
 215 
            hold('on');
- 
 
 216 
            plot(el(1), el(2), 'rx', 'MarkerSize', 15);
- 
 
 217 
        end
207 
    end
 218 
    end
208
219
209 
end
 220 
end
210 
 
 221 
 
211 
function [e, conf] = detectMarkersSubpix(frame, initGuesses, P, Q)
 222 
function [e, conf] = detectMarkersSubpix(frame, initGuesses, P, Q)
Line 217... Line 228...
217 
 
 228 
 
218 
    % project point cloud into image
 229 
    % project point cloud into image
219 
    q = [Q ones(size(Q,1),1)]*P;
 230 
    q = [Q ones(size(Q,1),1)]*P;
220 
    q = q./[q(:,3) q(:,3) q(:,3)];
 231 
    q = q./[q(:,3) q(:,3) q(:,3)];
221
232
- 
 
 233 
    e = zeros(size(initGuesses));
- 
 
 234 
    conf = zeros(size(initGuesses, 1), 1);
- 
 
 235
- 
 
 236 
    nMarkersFound = 0;
- 
 
 237
222 
    for i=1:size(initGuesses, 1)
 238 
    for i=1:size(initGuesses, 1)
223
239
224 
        labelId = labels(round(initGuesses(i,2)), round(initGuesses(i,1)));
 240 
        labelId = labels(round(initGuesses(i,2)), round(initGuesses(i,1)));
225 
        labelMask = (labels == labelId);
 241 
        labelMask = (labels == labelId);
226 
        labelMask = imdilate(labelMask, strel('disk', 3, 0));
 242 
        labelMask = imdilate(labelMask, strel('disk', 3, 0));
227
243
- 
 
 244 
        if(sum(sum(labelMask)) < 10 || sum(sum(labelMask)) > 1000)
- 
 
 245 
            continue;
- 
 
 246 
        end
- 
 
 247
228 
        % determine 3D points that are part of the marker
 248 
        % determine 3D points that are part of the marker
- 
 
 249 
        % note: we should probably undistort labelMask
229 
        pointMask = false(size(q, 1), 1);
 250 
        pointMask = false(size(q, 1), 1);
230 
        for j=1:size(q,1)
 251 
        for j=1:size(q,1)
231 
            if(labelMask(round(q(j,2)), round(q(j,1))))
 252 
            if(labelMask(round(q(j,2)), round(q(j,1))))
232 
                pointMask(j) = true;
 253 
                pointMask(j) = true;
233 
            end
 254 
            end
234 
        end
 255 
        end
235
256
- 
 
 257 
        if(sum(pointMask)) < 100
236 
        % build homography
 258 
            continue;
- 
 
 259 
        end
- 
 
 260
- 
 
 261 
        % project 3D points onto local plane
237 
        H = fitgeotrans(Q(pointMask, :), q(pointMask, :), 'projective');
 262 
        [~, sc, ~] = pca(Q(pointMask, :));
- 
 
 263 
        Qlocal = sc(:, 1:2);
- 
 
 264
- 
 
 265 
        % synthetic marker in high res. space
- 
 
 266 
        m = zeros(151, 151);
- 
 
 267 
        [x, y] = meshgrid(1:151, 1:151);
- 
 
 268 
        m((x(:)-76).^2 + (y(:)-76).^2 <= 50^2) = 1.0;
- 
 
 269
- 
 
 270 
        % relation between marker space (px) and true marker/local plane(mm)
- 
 
 271 
        % true marker diameter is 1.75mm
- 
 
 272 
        mScale = 101/1.75; %px/mm
- 
 
 273 
        mShift = 76; %px
238
274
- 
 
 275 
        % build homography from image to marker space
- 
 
 276 
        H = fitgeotrans(q(pointMask, 1:2), mScale*Qlocal+mShift,  'projective');
- 
 
 277
- 
 
 278 
        % bring image of marker into marker space
- 
 
 279 
        imMarkerSpace = imwarp(frame, H, 'OutputView', imref2d(size(m)));
- 
 
 280 
        imMarkerSpace = rgb2gray(im2double(imMarkerSpace));
- 
 
 281
- 
 
 282 
        %figure; imshowpair(imMarkerSpace, m);
- 
 
 283
- 
 
 284 
        % perform image registration
- 
 
 285 
        [opt, met] = imregconfig('monomodal');
- 
 
 286 
        T = imregtform(m, imMarkerSpace, 'translation', opt, met);
- 
 
 287
- 
 
 288 
        rege = imwarp(m, T, 'OutputView', imref2d(size(m)));
- 
 
 289 
        %figure; imshowpair(imMarkerSpace, rege);
- 
 
 290
- 
 
 291 
        % measure of correlation
- 
 
 292 
        confI = sum(sum(imMarkerSpace .* rege))/sqrt(sum(sum(imMarkerSpace) * sum(sum(rege))));
- 
 
 293
- 
 
 294 
        if confI<0.4
- 
 
 295 
            continue;
- 
 
 296 
        end
- 
 
 297
- 
 
 298 
        fprintf('Found marker with confidence: %f\n', confI);
- 
 
 299
- 
 
 300 
        % transform marker space coordinates (76,76) to frame space
- 
 
 301 
        el = T.transformPointsForward([76, 76]);
- 
 
 302 
        el = H.transformPointsInverse(el);
- 
 
 303
- 
 
 304 
        nMarkersFound = nMarkersFound+1;
- 
 
 305 
        e(nMarkersFound,:) = el;
- 
 
 306 
        conf(nMarkersFound) = confI;
239 
    end
 307 
    end
240
308
241 
    e = initGuesses;
 309 
    e = e(1:nMarkersFound, :);
242 
    conf = 1.0;
 310 
    conf = conf(1:nMarkersFound);
243 
 
 - 
 
244 
end
 311 
end
245 
 
 312 
 
246 
function E = projectOntoPointCloud(e, P, pointCloud)
 313 
function E = projectOntoPointCloud(e, P, pointCloud)
247 
 
 314 
 
248 
    q = [pointCloud ones(size(pointCloud,1),1)]*P;
 315 
    q = [pointCloud ones(size(pointCloud,1),1)]*P;