Subversion Repositories seema-scanner

Rev

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

Rev 215 Rev 216
Line 9... Line 9...
9 
 
 9 
 
10 
initialAlign = readMeshLabALN(alnFileName);
 10 
initialAlign = readMeshLabALN(alnFileName);
11 
 
 11 
 
12 
calibration = readOpenCVXML(calibrationFileName);
 12 
calibration = readOpenCVXML(calibrationFileName);
13 
 
 13 
 
- 
 
 14 
% correct for Matlab 1-indexing in principle point coordinates
- 
 
 15 
calibration.K0(1:2, 3) = calibration.K0(1:2, 3)+1;
- 
 
 16 
calibration.K1(1:2, 3) = calibration.K1(1:2, 3)+1;
- 
 
 17 
 
14 
% full projection matrices in Matlab convention
 18 
% full projection matrices in Matlab convention
15 
P0 = transpose(calibration.K0*[eye(3) zeros(3,1)]);
 19 
P0 = transpose(calibration.K0*[eye(3) zeros(3,1)]);
16 
P1 = transpose(calibration.K1*[calibration.R1 calibration.T1']);
 20 
P1 = transpose(calibration.K1*[calibration.R1 calibration.T1']);
17 
 
 21 
 
18 
% matlab cam params for undistortion
 22 
% matlab cam params for undistortion
Line 65... Line 69...
65 
        % should use pdist2 instead..
 69 
        % should use pdist2 instead..
66 
        sqDists = sum((E1 - repmat(E0(j,:), size(E1, 1), 1)).^2, 2);
 70 
        sqDists = sum((E1 - repmat(E0(j,:), size(E1, 1), 1)).^2, 2);
67
71
68 
        [minSqDist, minSqDistIdx] = min(sqDists);
 72 
        [minSqDist, minSqDistIdx] = min(sqDists);
69 
 
 73 
 
70 
        if(minSqDist < 5^2)
 74 
        if(minSqDist < 1^2)
71 
            nMatchedPairs = nMatchedPairs + 1;
 75 
            nMatchedPairs = nMatchedPairs + 1;
72 
            matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];
 76 
            matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];
73 
        end
 77 
        end
74 
    end
 78 
    end
75 
    matchedPairs = matchedPairs(1:nMatchedPairs, :);
 79 
    matchedPairs = matchedPairs(1:nMatchedPairs, :);
76
80
77 
    % triangulate marker centers (lens correction has been performed)
 81 
    % triangulate marker centers (lens correction has been performed)
78 
    [E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);
 82 
    [E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);
79 
    E{i} = E{i}(e<0.5, :);
 83 
    E{i} = E{i}(e<1.0, :);
- 
 
 84
- 
 
 85 
    % write point cloud with marker (debugging)
- 
 
 86 
    pcDebug = pointCloud([pc.Location; E{i}], 'Color', [pc.Color; repmat([255, 0, 0], size(E{i}, 1), 1)]);
- 
 
 87 
    pcwrite(pcDebug, 'pcDebug.ply');
80
88
81 
    % bring into initial alignment
 89 
    % bring markers into initial alignment
82 
    [U,~,V] = svd(initialAlign(i).Rotation);
 90 
    [U,~,V] = svd(initialAlign(i).Rotation);
83 
    Ri = U*V';
 91 
    Ri = U*V';
84 
    Ti = initialAlign(i).Translation;
 92 
    Ti = initialAlign(i).Translation;
85
93
86 
    Eg{i} = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);
 94 
    Eg{i} = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);
Line 324... Line 332...
324 
    E = nan(size(e,1), 3);
 332 
    E = nan(size(e,1), 3);
325
333
326 
    for i=1:size(e, 1)
 334 
    for i=1:size(e, 1)
327 
        sqDists = sum((q - repmat(e(i,:), size(q, 1), 1)).^2, 2);
 335 
        sqDists = sum((q - repmat(e(i,:), size(q, 1), 1)).^2, 2);
328
336
329 
        [minSqDist, minSqDistIdx] = min(sqDists);
 337 
        [sqDistsSorted, sortIdx] = sort(sqDists);
330
338
331 
        if(minSqDist < 2^2)
 339 
        neighbors = (sqDistsSorted < 4.0^2);
332
340
- 
 
 341 
        distsSorted = sqrt(sqDistsSorted(neighbors));
- 
 
 342 
        invDistsSorted = 1.0/distsSorted;
- 
 
 343 
        sortIdx = sortIdx(neighbors);
- 
 
 344
- 
 
 345 
        nNeighbors = sum(neighbors);
- 
 
 346
- 
 
 347 
        if(nNeighbors >= 2)
333 
            E(i, :) = pointCloud(minSqDistIdx, :);
 348 
            E(i, :) = 0;
- 
 
 349 
            for j=1:nNeighbors
- 
 
 350 
                E(i, :) = E(i, :) + invDistsSorted(j)/sum(invDistsSorted) * pointCloud(sortIdx(j), :);
334
351 
            end
335 
        end
 352 
        end
336
353
337 
    end
354 
    end
338 
end
 355 
end
339 
 
 356