Subversion Repositories seema-scanner

Rev

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

Rev 210 Rev 211
Line 5... Line 5...
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 
calibration = readOpenCVXML(calibrationFileName);
 - 
 
11 
initialAlign = readMeshLabALN(alnFileName);
 10 
initialAlign = readMeshLabALN(alnFileName);
12 
whiteFrameDirs = dir(fullfile(scanDir, 'sequence_*'));
 11 
whiteFrameDirs = dir(fullfile(scanDir, 'sequence_*'));
13 
 
 12 
 
14 
assert(length(whiteFrameDirs) == length(initialAlign));
 13 
assert(length(whiteFrameDirs) == length(initialAlign));
15 
 
 14 
 
- 
 
 15 
calibration = readOpenCVXML(calibrationFileName);
- 
 
 16 
 
- 
 
 17 
% full projection matrices in Matlab convention
- 
 
 18 
P0 = transpose(calibration.K0*[eye(3) zeros(3,1)]);
- 
 
 19 
P1 = transpose(calibration.K1*[calibration.R1 calibration.T1']);
- 
 
 20 
 
- 
 
 21 
% matlab cam params for undistortion
- 
 
 22 
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]));
- 
 
 24 
 
- 
 
 25 
% matlab struct for triangulation
- 
 
 26 
camStereoParams = stereoParameters(camParams0, camParams1, calibration.R1', calibration.T1');
- 
 
 27 
 
16 
nSubScans = length(whiteFrameDirs);
 28 
nSubScans = length(whiteFrameDirs);
17 
 
 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
- 
 
 34 
E = cell(nSubScans, 1);
- 
 
 35 
 
- 
 
 36 
% 3D coordinates of markers in global initial alignment
- 
 
 37 
Eg = cell(size(E));
- 
 
 38 
 
- 
 
 39 
% find 3D markers coordinates
18 
for i=1:nSubScans
 40 
for i=1:nSubScans
- 
 
 41 
 
- 
 
 42 
    % load point cloud
19 
    frame0 = imread(fullfile(whiteFrameDirs(i).folder, whiteFrameDirs(i).name, 'frames0_0.png'));
 43 
    pc = pcread(fullfile(scanDir, initialAlign(i).FileName));
- 
 
 44 
    Q = pc.Location;
20 
    frame1 = imread(fullfile(whiteFrameDirs(i).folder, whiteFrameDirs(i).name, 'frames1_0.png'));
 45 
    idString = initialAlign(i).FileName(12:end-4);
21
46
22 
    %BW0 = im2bw(frame0, graythresh(frame0));
 47 
    % load white frames
23 
    %RP0 = regionprops(BW0, 'Area', 'Solidity', 'Eccentricity', 'Orientation', 'Centroid');
 48 
    frame0 = imread(fullfile(scanDir, ['sequence_' idString], 'frames0_0.png'));
- 
 
 49 
    frame1 = imread(fullfile(scanDir, ['sequence_' idString], 'frames1_0.png'));
24
50 
 
25 
    gray0 = rgb2gray(frame0);
 51 
%     e0Coords = autoDetectEllipses(frame0, ep);
- 
 
 52 
%     e1Coords = autoDetectEllipses(frame1, ep);
26
53
- 
 
 54 
    e0Coords = manuallyDetectEllipses(frame0, ep);
27 
	[edge0, tr] = edge(gray0, 'Canny', [0.08 0.1], 2);
 55 
    e1Coords = manuallyDetectEllipses(frame1, ep);
28
56
- 
 
 57 
    e0Coords = undistortPoints(e0Coords, camParams0);
- 
 
 58 
    e1Coords = undistortPoints(e1Coords, camParams1);
- 
 
 59 
 
- 
 
 60 
    % match ellipse candidates between cameras based on projection
- 
 
 61 
    E0 = projectOntoPointCloud(e0Coords, P0, Q);
- 
 
 62 
    E1 = projectOntoPointCloud(e1Coords, P1, Q);
- 
 
 63 
 
29 
    %edge0 = edge0(1190:1290, 1975:2045);
 64 
    matchedPairs = nan(size(E0, 1), 2);
30 
    %figure; imagesc(edge0);
 65 
    nMatchedPairs = 0;
- 
 
 66 
    for j=1:size(E0, 1)
- 
 
 67
- 
 
 68 
        % should use pdist2 instead..
- 
 
 69 
        sqDists = sum((E1 - repmat(E0(j,:), size(E1, 1), 1)).^2, 2);
- 
 
 70
- 
 
 71 
        [minSqDist, minSqDistIdx] = min(sqDists);
- 
 
 72 
 
- 
 
 73 
        if(minSqDist < 5^2)
- 
 
 74 
            nMatchedPairs = nMatchedPairs + 1;
- 
 
 75 
            matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];
- 
 
 76 
        end
- 
 
 77 
    end
- 
 
 78 
    matchedPairs = matchedPairs(1:nMatchedPairs, :);
31
79
- 
 
 80 
    % triangulate marker centers (lens correction has been performed)
32 
    ep = struct('minMajorAxis', 25, 'maxMajorAxis', 35, 'minAspectRatio', 0.4, 'numBest', 10, 'randomize', 2, 'smoothStddev', 1);
 81 
    E{i} = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);
- 
 
 82
- 
 
 83 
    % bring into initial alignment
- 
 
 84 
    [U,~,V] = svd(initialAlign(i).Rotation);
- 
 
 85 
    Ri = U*V';
33 
    e0 = ellipseDetection(edge0, ep);
 86 
    Ti = initialAlign(i).Translation;
34
87
- 
 
 88 
    Eg{i} = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);
- 
 
 89 
end
- 
 
 90 
 
- 
 
 91 
% match markers between poses using initial alignment
- 
 
 92 
 
- 
 
 93 
for i=1:nSubScans
- 
 
 94 
    % fix Ri to be orthogonal
- 
 
 95 
    [U,~,V] = svd(initialAlign(i).Rotation);
- 
 
 96 
    Ri = U*V';
- 
 
 97
- 
 
 98 
    % bring point cloud into initial alignment
- 
 
 99 
    pc = pcread(fullfile(scanDir, initialAlign(i).FileName));
- 
 
 100 
    tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);
- 
 
 101 
    pcg = pctransform(pc, tform);
- 
 
 102
- 
 
 103 
    figure;
- 
 
 104 
    hold('on');
35 
    figure; hold('on'); imshow(edge0); ellipse(e0(:,3), e0(:,4), e0(:,5)*pi/180, e0(:,1), e0(:,2), 'r');
105 
	plot3(Eg{i}(:,1), Eg{i}(:,2), Eg{i}(:,3), 'r.', 'MarkerSize', 15);
- 
 
 106 
    pcshow(pcg);
- 
 
 107 
    xlabel('x');
- 
 
 108 
    ylabel('y');
- 
 
 109 
    zlabel('z');
- 
 
 110 
end
- 
 
 111 
 
- 
 
 112 
 
- 
 
 113 
end
- 
 
 114 
 
- 
 
 115 
function e = autoDetectEllipses(frame, ep)
- 
 
 116 
 
- 
 
 117 
    % create mask based on morphology
- 
 
 118 
    bw = imbinarize(rgb2gray(frame));
- 
 
 119 
    cc = bwconncomp(bw);
- 
 
 120 
    rp = regionprops(cc, 'Area', 'Solidity', 'Eccentricity', 'Centroid');
- 
 
 121 
    idx = find([rp.Area] > 100 & [rp.Area] < 1000 & [rp.Solidity] > 0.9);
- 
 
 122 
    mask = ismember(labelmatrix(cc0), idx);
- 
 
 123 
    mask = imdilate(mask, strel('disk', 20, 0));
- 
 
 124
- 
 
 125 
    % detect ellipses within mask
- 
 
 126 
    edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);
- 
 
 127 
    edges(~mask) = 0;
- 
 
 128 
    ep.numBest = 10;
- 
 
 129 
    e = ellipseDetection(edges, ep);
- 
 
 130 
end
- 
 
 131 
 
- 
 
 132 
function e = manuallyDetectEllipses(frame, ep)
36
133
- 
 
 134 
    figure;
- 
 
 135 
    imshow(frame);
- 
 
 136 
    title('Press return to end');
- 
 
 137 
 
- 
 
 138 
    p = ginput();
- 
 
 139 
 
- 
 
 140 
    e = [];
- 
 
 141 
    for i=1:size(p, 1)
- 
 
 142 
        % create mask around selected point
- 
 
 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));
- 
 
 147 
 
- 
 
 148 
        % detect ellipses within mask
- 
 
 149 
        edges = edge(rgb2gray(frame), 'Canny', [0.08 0.1], 2);
- 
 
 150 
        edges(~mask) = 0;
- 
 
 151 
 
- 
 
 152 
        ep.numBest = 1;
- 
 
 153 
        el = ellipseDetection(edges, ep);
- 
 
 154 
 
- 
 
 155 
        e = [e; el(:, 1:2)];
- 
 
 156 
    end
37
157
- 
 
 158 
    close(gcf);
38
159
39 
end
 160 
end
40 
 
 161 
 
- 
 
 162 
function E = projectOntoPointCloud(e, P, Q)
- 
 
 163 
 
- 
 
 164 
    q = [Q ones(size(Q,1),1)]*P;
- 
 
 165 
    q = q(:,1:2)./[q(:,3) q(:,3)];
- 
 
 166 
 
- 
 
 167 
    E = nan(size(e,1), 3);
- 
 
 168
- 
 
 169 
    for i=1:size(e, 1)
- 
 
 170 
        sqDists = sum((q - repmat(e(i,:), size(q, 1), 1)).^2, 2);
- 
 
 171
- 
 
 172 
        [minSqDist, minSqDistIdx] = min(sqDists);
- 
 
 173
- 
 
 174 
        if(minSqDist < 2^2)
- 
 
 175
- 
 
 176 
            E(i, :) = Q(minSqDistIdx, :);
- 
 
 177
- 
 
 178 
        end
- 
 
 179
- 
 
 180 
    end
41 
end
 181 
end
42 
 
 182