Subversion Repositories seema-scanner

Rev

Rev 212 | Rev 214 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 212 Rev 213
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 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 
whiteFrameDirs = dir(fullfile(scanDir, 'sequence_*'));
 11 
whiteFrameDirs = dir(fullfile(scanDir, 'sequence_*'));
12 
 
 12 
 
13 
assert(length(whiteFrameDirs) == length(initialAlign));
 13 
assert(length(whiteFrameDirs) == length(initialAlign));
14 
 
 14 
 
15 
calibration = readOpenCVXML(calibrationFileName);
 15 
calibration = readOpenCVXML(calibrationFileName);
16 
 
 16 
 
17 
% full projection matrices in Matlab convention
 17 
% full projection matrices in Matlab convention
18 
P0 = transpose(calibration.K0*[eye(3) zeros(3,1)]);
 18 
P0 = transpose(calibration.K0*[eye(3) zeros(3,1)]);
19 
P1 = transpose(calibration.K1*[calibration.R1 calibration.T1']);
 19 
P1 = transpose(calibration.K1*[calibration.R1 calibration.T1']);
20 
 
 20 
 
21 
% matlab cam params for undistortion
 21 
% matlab cam params for undistortion
22 
camParams0 = cameraParameters('IntrinsicMatrix', calibration.K0', 'RadialDistortion', calibration.k0([1 2 5]), 'TangentialDistortion', calibration.k0([3 4]));
 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]));
 23 
camParams1 = cameraParameters('IntrinsicMatrix', calibration.K1', 'RadialDistortion', calibration.k1([1 2 5]), 'TangentialDistortion', calibration.k1([3 4]));
24 
 
 24 
 
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));
38 
 
 35 
 
39 
% find 3D markers coordinates
36 
% find 3D markers coordinates
40 
for i=1:nSubScans
 37 
for i=1:nSubScans
41 
 
 38 
 
42 
    % load point cloud
 39 
    % load point cloud
43 
    pc = pcread(fullfile(scanDir, initialAlign(i).FileName));
 40 
    pc = pcread(fullfile(scanDir, initialAlign(i).FileName));
44 
    Q = pc.Location;
 41 
    Q = pc.Location;
45 
    idString = initialAlign(i).FileName(12:end-4);
 42 
    idString = initialAlign(i).FileName(12:end-4);
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
61 
    E0 = projectOntoPointCloud(e0Coords, P0, Q);
 58 
    E0 = projectOntoPointCloud(e0Coords, P0, Q);
62 
    E1 = projectOntoPointCloud(e1Coords, P1, Q);
 59 
    E1 = projectOntoPointCloud(e1Coords, P1, Q);
63 
 
 60 
 
64 
    matchedPairs = nan(size(E0, 1), 2);
 61 
    matchedPairs = nan(size(E0, 1), 2);
65 
    nMatchedPairs = 0;
 62 
    nMatchedPairs = 0;
66 
    for j=1:size(E0, 1)
 63 
    for j=1:size(E0, 1)
67
64
68 
        % should use pdist2 instead..
 65 
        % should use pdist2 instead..
69 
        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);
70
67
71 
        [minSqDist, minSqDistIdx] = min(sqDists);
 68 
        [minSqDist, minSqDistIdx] = min(sqDists);
72 
 
 69 
 
73 
        if(minSqDist < 5^2)
 70 
        if(minSqDist < 5^2)
74 
            nMatchedPairs = nMatchedPairs + 1;
 71 
            nMatchedPairs = nMatchedPairs + 1;
75 
            matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];
 72 
            matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];
76 
        end
 73 
        end
77 
    end
 74 
    end
78 
    matchedPairs = matchedPairs(1:nMatchedPairs, :);
 75 
    matchedPairs = matchedPairs(1:nMatchedPairs, :);
79
76
80 
    % triangulate marker centers (lens correction has been performed)
 77 
    % triangulate marker centers (lens correction has been performed)
81 
    E{i} = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);
 78 
    E{i} = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);
82
79
83 
    % bring into initial alignment
 80 
    % bring into initial alignment
84 
    [U,~,V] = svd(initialAlign(i).Rotation);
 81 
    [U,~,V] = svd(initialAlign(i).Rotation);
85 
    Ri = U*V';
 82 
    Ri = U*V';
86 
    Ti = initialAlign(i).Translation;
 83 
    Ti = initialAlign(i).Translation;
87
84
88 
    Eg{i} = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);
 85 
    Eg{i} = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);
89 
end
 86 
end
90 
 
 87 
 
91 
% show found markers in initial alignment
 88 
% show found markers in initial alignment
92 
figure;
 89 
figure;
93 
hold('on');
 90 
hold('on');
94 
for i=1:nSubScans
 91 
for i=1:nSubScans
95 
    % fix Ri to be orthogonal
 92 
    % fix Ri to be orthogonal
96 
    [U,~,V] = svd(initialAlign(i).Rotation);
 93 
    [U,~,V] = svd(initialAlign(i).Rotation);
97 
    Ri = U*V';
 94 
    Ri = U*V';
98
95
99 
    % bring point cloud into initial alignment
 96 
    % bring point cloud into initial alignment
100 
    pc = pcread(fullfile(scanDir, initialAlign(i).FileName));
 97 
    pc = pcread(fullfile(scanDir, initialAlign(i).FileName));
101 
    tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);
 98 
    tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);
102 
    pcg = pctransform(pc, tform);
 99 
    pcg = pctransform(pc, tform);
103
100
104 
    pcshow(pcg);
 101 
    pcshow(pcg);
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 = {};
115 
for i=1:nSubScans
 113 
for i=1:nSubScans
116 
    for j=1:size(Eg{i}, 1)
 114 
    for j=1:size(Eg{i}, 1)
117 
        pg = Eg{i}(j,:);
 115 
        pg = Eg{i}(j,:);
118 
        p = E{i}(j,:);
 116 
        p = E{i}(j,:);
119 
        matched = false;
 117 
        matched = false;
120 
        for k=1:size(Pg, 2)
 118 
        for k=1:size(Pg, 2)
121 
            clusterCenter = mean(cat(1, Pg{:,k}), 1);
 119 
            clusterCenter = mean(cat(1, Pg{:,k}), 1);
122 
            if(sum((pg - clusterCenter).^2) < 3^2)
 120 
            if(sum((pg - clusterCenter).^2) < 3^2)
123 
                % store in global frame
 121 
                % store in global frame
124 
                Pg{i,k} = pg;
 122 
                Pg{i,k} = pg;
125 
                % store in local frame
 123 
                % store in local frame
126 
                P{i,k} = p;
 124 
                P{i,k} = p;
127 
                matched = true;
 125 
                matched = true;
128 
                break;
 126 
                break;
129 
            end
 127 
            end
130 
        end
 128 
        end
131 
        % create new cluster
 129 
        % create new cluster
132 
        if(not(matched))
 130 
        if(not(matched))
133 
            Pg{i,end+1} = pg;
 131 
            Pg{i,end+1} = pg;
134 
            P{i,end+1} = p;
 132 
            P{i,end+1} = p;
135 
        end
133 
        end
136 
    end
 134 
    end
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)
212 
 
 223 
 
213 
    % create mask based on morphology
 224 
    % create mask based on morphology
214 
    bw = imbinarize(rgb2gray(frame));
 225 
    bw = imbinarize(rgb2gray(frame));
215 
    cc = bwconncomp(bw);
 226 
    cc = bwconncomp(bw);
216 
    labels = labelmatrix(cc);
 227 
    labels = labelmatrix(cc);
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;
249 
    q = q(:,1:2)./[q(:,3) q(:,3)];
 316 
    q = q(:,1:2)./[q(:,3) q(:,3)];
250 
 
 317 
 
251 
    E = nan(size(e,1), 3);
 318 
    E = nan(size(e,1), 3);
252
319
253 
    for i=1:size(e, 1)
 320 
    for i=1:size(e, 1)
254 
        sqDists = sum((q - repmat(e(i,:), size(q, 1), 1)).^2, 2);
 321 
        sqDists = sum((q - repmat(e(i,:), size(q, 1), 1)).^2, 2);
255
322
256 
        [minSqDist, minSqDistIdx] = min(sqDists);
 323 
        [minSqDist, minSqDistIdx] = min(sqDists);
257
324
258 
        if(minSqDist < 2^2)
 325 
        if(minSqDist < 2^2)
259
326
260 
            E(i, :) = pointCloud(minSqDistIdx, :);
 327 
            E(i, :) = pointCloud(minSqDistIdx, :);
261
328
262 
        end
 329 
        end
263
330
264 
    end
331 
    end
265 
end
 332 
end
266 
 
 333 
 
267 
 
 334