Subversion Repositories seema-scanner

Rev

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

Rev 236 Rev 237
Line 6... Line 6...
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 
[alnFilePath, ~, ~] = fileparts(alnFileName);
11 
 
 12 
 
12 
calibration = readOpenCVXML(calibrationFileName);
 13 
calibration = readOpenCVXML(calibrationFileName);
13 
 
 14 
 
14 
% correct for Matlab 1-indexing in principle point coordinates
 15 
% correct for Matlab 1-indexing in principle point coordinates
15 
calibration.K0(1:2, 3) = calibration.K0(1:2, 3)+1;
 16 
calibration.K0(1:2, 3) = calibration.K0(1:2, 3)+1;
Line 36... Line 37...
36 
 
 37 
 
37 
% find 3D markers coordinates
38 
% find 3D markers coordinates
38 
for i=1:nSubScans
 39 
for i=1:nSubScans
39 
%for i=5:5
 40 
%for i=5:5
40 
    % load point cloud
 41 
    % load point cloud
- 
 
 42 
    pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);
- 
 
 43 
    pcFilePath = fileparts(pcFileName);
41 
    pc = pcread(initialAlign(i).FileName);
 44 
    pc = pcread(pcFileName);
42 
    Q = pc.Location;
 45 
    Q = pc.Location;
43 
    idString = strsplit(initialAlign(i).FileName, {'.ply', '_'});
 46 
    idString = strsplit(initialAlign(i).FileName, {'.ply', '_'});
44 
    idString = idString{end-1};
 47 
    idString = idString{end-1};
45
48
46 
    % load white frames
 49 
    % load white frames
- 
 
 50 
    frame0 = imread(fullfile(pcFilePath, ['sequence_' idString], 'frames0_0.png'));
- 
 
 51 
    frame1 = imread(fullfile(pcFilePath, ['sequence_' idString], 'frames1_0.png'));
- 
 
 52 
 
- 
 
 53 
    e0Coords = autoDetectMarkers(frame0);
- 
 
 54 
    e1Coords = autoDetectMarkers(frame1);
- 
 
 55
47 
    scanDir = strsplit(initialAlign(i).FileName, '/');
 56 
    %e0Coords = manuallyDetectMarkers(frame0);
48 
    scanDir = fullfile(scanDir{1:end-1});
 57 
    %e1Coords = manuallyDetectMarkers(frame1);
- 
 
 58
49 
    frame0 = imread(fullfile(scanDir, ['sequence_' idString], 'frames0_0.png'));
 59 
    %[e0Coords, conf0] = detectMarkersSubpix(frame0, e0Coords, P0, Q);
50 
    frame1 = imread(fullfile(scanDir, ['sequence_' idString], 'frames1_0.png'));
 60 
    %[e1Coords, conf1] = detectMarkersSubpix(frame1, e1Coords, P1, Q);
51 
 
 61 
 
52 
    e0Coords = autoDetectMarkers(frame0, P0, Q);
 62 
    if(length(e0Coords) < 1 || length(e1Coords) < 1)
53 
    e1Coords = autoDetectMarkers(frame1, P1, Q);
 63 
        continue;
- 
 
 64 
    end
54
65
- 
 
 66 
%     figure;
- 
 
 67 
%     subplot(1,2,1);
- 
 
 68 
%     imshow(frame0);
- 
 
 69 
%     hold('on');
55 
    %e0Coords = manuallyDetectMarkers(frame0, P0, Q);
 70 
%     plot(e0Coords(:,1), e0Coords(:,2), 'rx', 'MarkerSize', 15);
- 
 
 71 
%     subplot(1,2,2);
- 
 
 72 
%     imshow(frame1);
- 
 
 73 
%     hold('on');
56 
    %e1Coords = manuallyDetectMarkers(frame1, P1, Q);
 74 
%     plot(e1Coords(:,1), e1Coords(:,2), 'rx', 'MarkerSize', 15);
- 
 
 75 
%     drawnow;
57
76
58 
    e0Coords = undistortPoints(e0Coords, camParams0);
 77 
    e0Coords = undistortPoints(e0Coords, camParams0);
59 
    e1Coords = undistortPoints(e1Coords, camParams1);
 78 
    e1Coords = undistortPoints(e1Coords, camParams1);
60 
 
 79 
 
61 
    % match ellipse candidates between cameras based on projection
 80 
    % match ellipse candidates between cameras based on projection
Line 76... Line 95...
76 
            matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];
 95 
            matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];
77 
        end
 96 
        end
78 
    end
 97 
    end
79 
    matchedPairs = matchedPairs(1:nMatchedPairs, :);
 98 
    matchedPairs = matchedPairs(1:nMatchedPairs, :);
80
99
- 
 
 100 
    figure;
- 
 
 101 
    subplot(1,2,1);
- 
 
 102 
    imshow(frame0);
- 
 
 103 
    hold('on');
- 
 
 104 
    plot(e0Coords(matchedPairs(:, 1),1), e0Coords(matchedPairs(:, 1),2), 'rx', 'MarkerSize', 15);
- 
 
 105 
    subplot(1,2,2);
- 
 
 106 
    imshow(frame1);
- 
 
 107 
    hold('on');
- 
 
 108 
    plot(e1Coords(matchedPairs(:, 2),1), e1Coords(matchedPairs(:, 2),2), 'rx', 'MarkerSize', 15);
- 
 
 109 
    drawnow;
- 
 
 110
81 
    % triangulate marker centers (lens correction has been performed)
 111 
    % triangulate marker centers (lens correction has been performed)
82 
    [E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);
 112 
    [E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);
83 
    E{i} = E{i}(e<3.0, :);
 113 
    E{i} = E{i}(e<3.0, :);
84 
    display(e);
 114 
    display(e);
85
115
Line 102... Line 132...
102 
    % fix Ri to be orthogonal
 132 
    % fix Ri to be orthogonal
103 
    [U,~,V] = svd(initialAlign(i).Rotation);
 133 
    [U,~,V] = svd(initialAlign(i).Rotation);
104 
    Ri = U*V';
 134 
    Ri = U*V';
105
135
106 
    % bring point cloud into initial alignment
 136 
    % bring point cloud into initial alignment
- 
 
 137 
    pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);
107 
    pc = pcread(initialAlign(i).FileName);
 138 
    pc = pcread(pcFileName);
108 
    tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);
 139 
    tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);
109 
    pcg = pctransform(pc, tform);
 140 
    pcg = pctransform(pc, tform);
110
141
111 
    pcshow(pcg);
 142 
    pcshow(pcg);
112 
    xlabel('x');
 143 
    xlabel('x');
Line 183... Line 214...
183 
 
 214 
 
184 
function e = autoDetectMarkers(frame, P, pointCloud)
 215 
function e = autoDetectMarkers(frame, P, pointCloud)
185 
 
 216 
 
186 
    % create mask based on morphology
 217 
    % create mask based on morphology
187 
    g = rgb2gray(frame);
 218 
    g = rgb2gray(frame);
188 
    g(g>254) = 0;
 219 
    % g(g>254) = 0;
189 
    bw = imbinarize(g, 'adaptive', 'Sensitivity', 10^(-50));
 220 
    % bw = imbinarize(g, 'adaptive', 'Sensitivity', 10^(-50));
- 
 
 221 
    bw = imbinarize(g, 0.10);
190 
    cc = bwconncomp(bw);
 222 
    cc = bwconncomp(bw);
191 
    rp = regionprops(cc, 'Area', 'Solidity', 'Eccentricity', 'Centroid');
 223 
    rp = regionprops(cc, 'Area', 'Solidity', 'Eccentricity', 'Centroid');
192 
    idx = ([rp.Area] > 100 & [rp.Area] < 1000 & [rp.Solidity] > 0.9);
 224 
    idx = ([rp.Area] > 100 & [rp.Area] < 1000 & [rp.Solidity] > 0.9);
193
225
194 
    initialGuesses = cat(1, rp(idx).Centroid);
 226 
    e = cat(1, rp(idx).Centroid);
195 
 
 227 
 
196 
    [e, ~] = detectMarkersSubpix(frame, initialGuesses, P, pointCloud);
 - 
 
197 
 
 - 
 
198 
    figure;
- 
 
199 
    imshow(frame);
 - 
 
200 
        hold('on');
 - 
 
201 
    plot(e(:,1), e(:,2), 'rx', 'MarkerSize', 15);
 - 
 
202 
    drawnow;
 - 
 
203 
end
 228 
end
204 
 
 229 
 
205 
function e = manuallyDetectMarkers(frame, P, pointCloud)
 230 
function e = manuallyDetectMarkers(frame, P, pointCloud)
206
231
207 
    e = [];
 232 
    e = [];
Line 219... Line 244...
219 
    function clickCallback(caller, ~)
 244 
    function clickCallback(caller, ~)
220
245
221 
        p = get(gca, 'CurrentPoint');
246 
        p = get(gca, 'CurrentPoint');
222 
        p = p(1, 1:2);
 247 
        p = p(1, 1:2);
223 
 
 248 
 
224 
        [el, ~] = detectMarkersSubpix(frame, p, P, pointCloud);
 - 
 
225 
        e = [e; el(:, 1:2)];
 249 
        e = [e; p(:, 1:2)];
226
250
227 
        if(not(isempty(el)))
 251 
        if(not(isempty(el)))
228 
            figure(caller);
 252 
            figure(caller);
229 
            hold('on');
 253 
            hold('on');
230 
            plot(el(1), el(2), 'rx', 'MarkerSize', 15);
 254 
            plot(el(1), el(2), 'rx', 'MarkerSize', 15);
Line 232... Line 256...
232 
    end
 256 
    end
233
257
234 
end
 258 
end
235 
 
 259 
 
236 
function [e, conf] = detectMarkersSubpix(frame, initGuesses, P, Q)
 260 
function [e, conf] = detectMarkersSubpix(frame, initGuesses, P, Q)
- 
 
 261 
% Detect a marker in a single frame by rectifying to the image and
- 
 
 262 
% performing image registration.
237 
 
 263 
 
238 
    % create mask based on morphology
 264 
    % create mask based on morphology
239 
    g = rgb2gray(frame);
 265 
    g = rgb2gray(frame);
240 
    g(g>254) = 0;
 266 
    g(g>254) = 0;
241 
    bw = imbinarize(g);
 267 
    bw = imbinarize(g);
Line 333... Line 359...
333 
    e = e(1:nMarkersFound, :);
 359 
    e = e(1:nMarkersFound, :);
334 
    conf = conf(1:nMarkersFound);
 360 
    conf = conf(1:nMarkersFound);
335 
end
 361 
end
336 
 
 362 
 
337 
function E = projectOntoPointCloud(e, P, pointCloud)
 363 
function E = projectOntoPointCloud(e, P, pointCloud)
- 
 
 364 
% Project 2d point coordinates onto pointCloud to find corresponding 3d
- 
 
 365 
% point coordinates.
338 
 
 366 
 
339 
    q = [pointCloud ones(size(pointCloud,1),1)]*P;
 367 
    q = [pointCloud ones(size(pointCloud,1),1)]*P;
340 
    q = q(:,1:2)./[q(:,3) q(:,3)];
 368 
    q = q(:,1:2)./[q(:,3) q(:,3)];
341 
 
 369 
 
342 
    E = nan(size(e,1), 3);
 370 
    E = nan(size(e,1), 3);