Subversion Repositories seema-scanner

Rev

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

Rev 240 Rev 241
Line 26... Line 26...
26 
 
 26 
 
27 
% matlab struct for triangulation
 27 
% matlab struct for triangulation
28 
camStereoParams = stereoParameters(camParams0, camParams1, calibration.R1', calibration.T1');
 28 
camStereoParams = stereoParameters(camParams0, camParams1, calibration.R1', calibration.T1');
29 
 
 29 
 
30 
nSubScans = length(initialAlign);
 30 
nSubScans = length(initialAlign);
- 
 
 31 
%nSubScans = 5;
31 
 
 32 
 
32 
% 3D coordinates of markers in local camera frame
 33 
% 3D coordinates of markers in local camera frame
33 
E = cell(nSubScans, 1);
 34 
E = cell(nSubScans, 1);
34 
 
 35 
 
35 
% 3D coordinates of markers in global initial alignment
 36 
% 3D coordinates of markers in global initial alignment
36 
Eg = cell(size(E));
 37 
Eg = cell(size(E));
37 
 
 38 
 
38 
% find 3D markers coordinates
39 
% find 3D markers coordinates
39 
for i=1:nSubScans
 40 
for i=1:nSubScans
40 
%for i=5:5
 - 
 
41 
    % load point cloud
 41 
    % load point cloud
42 
    pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);
 42 
    pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);
43 
    pcFilePath = fileparts(pcFileName);
 43 
    pcFilePath = fileparts(pcFileName);
44 
    pc = pcread(pcFileName);
 44 
    pc = pcread(pcFileName);
45 
    Q = pc.Location;
 45 
    Q = pc.Location;
Line 72... Line 72...
72 
%     imshow(frame1);
 72 
%     imshow(frame1);
73 
%     hold('on');
 73 
%     hold('on');
74 
%     plot(e1Coords(:,1), e1Coords(:,2), 'rx', 'MarkerSize', 15);
 74 
%     plot(e1Coords(:,1), e1Coords(:,2), 'rx', 'MarkerSize', 15);
75 
%     drawnow;
 75 
%     drawnow;
76
76
77 
    e0Coords = undistortPointsFast(e0Coords, camParams0);
 77 
    e0CoordsUndistort = undistortPointsFast(e0Coords, camParams0);
78 
    e1Coords = undistortPointsFast(e1Coords, camParams1);
 78 
    e1CoordsUndistort = undistortPointsFast(e1Coords, camParams1);
79 
 
 79 
 
80 
    % match ellipse candidates between cameras based on projection
 80 
    % match ellipse candidates between cameras based on projection
81 
    E0 = projectOntoPointCloud(e0Coords, P0, Q);
 81 
    E0 = projectOntoPointCloud(e0CoordsUndistort, P0, Q);
82 
    E1 = projectOntoPointCloud(e1Coords, P1, Q);
 82 
    E1 = projectOntoPointCloud(e1CoordsUndistort, P1, Q);
83 
 
 83 
 
84 
    matchedPairs = nan(size(E0, 1), 2);
 84 
    matchedPairs = nan(size(E0, 1), 2);
85 
    nMatchedPairs = 0;
 85 
    nMatchedPairs = 0;
86 
    for j=1:size(E0, 1)
 86 
    for j=1:size(E0, 1)
87
87
88 
        % should use pdist2 instead..
 88 
        % should use pdist2 instead..
89 
        sqDists = sum((E1 - repmat(E0(j,:), size(E1, 1), 1)).^2, 2);
 89 
        sqDists = sum((E1 - repmat(E0(j,:), size(E1, 1), 1)).^2, 2);
90
90
91 
        [minSqDist, minSqDistIdx] = min(sqDists);
 91 
        [minSqDist, minSqDistIdx] = min(sqDists);
92 
 
 92 
 
93 
        if(minSqDist < 1^2)
 93 
        if(minSqDist < 2^2)
94 
            nMatchedPairs = nMatchedPairs + 1;
 94 
            nMatchedPairs = nMatchedPairs + 1;
95 
            matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];
 95 
            matchedPairs(nMatchedPairs, :) = [j, minSqDistIdx];
96 
        end
 96 
        end
97 
    end
 97 
    end
98 
    matchedPairs = matchedPairs(1:nMatchedPairs, :);
 98 
    matchedPairs = matchedPairs(1:nMatchedPairs, :);
Line 115... Line 115...
115
115
116 
    [E{i}, e] = detectMarkersStereoSubpix(frame0, frame1, E0(matchedPairs(:, 1), :), camStereoParams, pc);
 116 
    [E{i}, e] = detectMarkersStereoSubpix(frame0, frame1, E0(matchedPairs(:, 1), :), camStereoParams, pc);
117 
    display(e);
 117 
    display(e);
118
118
119 
    % write point cloud with marker (debugging)
 119 
    % write point cloud with marker (debugging)
120 
    pcDebug = pointCloud([pc.Location; E{i}], 'Color', [pc.Color; repmat([255, 0, 0], size(E{i}, 1), 1)]);
 120 
    %pcDebug = pointCloud([pc.Location; E{i}], 'Color', [pc.Color; repmat([255, 0, 0], size(E{i}, 1), 1)]);
121 
    pcwrite(pcDebug, 'pcDebug.ply');
 121 
    %pcwrite(pcDebug, 'pcDebug.ply');
122
122
123 
    % bring markers into initial alignment
 123 
    % bring markers into initial alignment
124 
    [U,~,V] = svd(initialAlign(i).Rotation);
 124 
    [U,~,V] = svd(initialAlign(i).Rotation);
125 
    Ri = U*V';
 125 
    Ri = U*V';
126 
    Ti = initialAlign(i).Translation;
 126 
    Ti = initialAlign(i).Translation;
Line 133... Line 133...
133 
hold('on');
 133 
hold('on');
134 
for i=1:nSubScans
 134 
for i=1:nSubScans
135 
    % fix Ri to be orthogonal
 135 
    % fix Ri to be orthogonal
136 
    [U,~,V] = svd(initialAlign(i).Rotation);
 136 
    [U,~,V] = svd(initialAlign(i).Rotation);
137 
    Ri = U*V';
 137 
    Ri = U*V';
- 
 
 138 
    initialAlign(i).Rotation = Ri;
138
139
139 
    % bring point cloud into initial alignment
 140 
    % bring point cloud into initial alignment
140 
    pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);
 141 
    pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);
141 
    pc = pcread(pcFileName);
 142 
    pc = pcread(pcFileName);
142 
    tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);
 143 
    tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);
Line 184... Line 185...
184 
% show found markers in optimized alignment
 185 
% show found markers in optimized alignment
185 
figure;
 186 
figure;
186 
hold('on');
 187 
hold('on');
187 
for i=1:nSubScans
 188 
for i=1:nSubScans
188 
    % fix Ri to be orthogonal
 189 
    % fix Ri to be orthogonal
189 
    [U,~,V] = svd(alignment(i).Rotation);
 190 
    %[U,~,V] = svd(alignment(i).Rotation);
190 
    Ri = U*V';
 191 
    %Ri = U*V';
- 
 
 192 
    Ri = alignment(i).Rotation;
191 
    Ti = alignment(i).Translation;
 193 
    Ti = alignment(i).Translation;
192
194
193 
    Ea = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);
 195 
    Ea = E{i}*Ri' + repmat(Ti', size(E{i}, 1), 1);
194
196
195 
    % bring point cloud into optimized alignment
 197 
    % bring point cloud into optimized alignment
- 
 
 198 
    pcFileName = fullfile(alnFilePath, initialAlign(i).FileName);
196 
    pc = pcread(initialAlign(i).FileName);
 199 
    pc = pcread(pcFileName);
197 
    tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);
 200 
    tform = affine3d([Ri' [0;0;0]; initialAlign(i).Translation' 1]);
198 
    pcg = pctransform(pc, tform);
 201 
    pcg = pctransform(pc, tform);
199
202
200 
    pcshow(pcg);
 203 
    pcshow(pcg);
201 
    xlabel('x');
 204 
    xlabel('x');
Line 370... Line 373...
370 
    normals = pcnormals(pc, 6);
 373 
    normals = pcnormals(pc, 6);
371
374
372 
    frame0 = rgb2gray(frame0);
 375 
    frame0 = rgb2gray(frame0);
373 
    frame1 = rgb2gray(frame1);
 376 
    frame1 = rgb2gray(frame1);
374 
 
 377 
 
375 
    nMarkers = size(initGuesses, 2);
 378 
    nMarkers = size(initGuesses, 1);
376
379
377 
    E = zeros(nMarkers, 3);
 380 
    E = zeros(nMarkers, 3);
378 
    conf = zeros(nMarkers, 1);
 381 
    conf = zeros(nMarkers, 1);
379
382
380 
    for i=1:nMarkers
 383 
    for i=1:nMarkers
Line 398... Line 401...
398 
        e0ImCoords = [x(:), y(:)];
 401 
        e0ImCoords = [x(:), y(:)];
399
402
400 
        [x,y] = meshgrid(e1Center(1)-margin:e1Center(1)+margin, e1Center(2)-margin:e1Center(2)+margin);
 403 
        [x,y] = meshgrid(e1Center(1)-margin:e1Center(1)+margin, e1Center(2)-margin:e1Center(2)+margin);
401 
        e1ImCoords = [x(:), y(:)];
 404 
        e1ImCoords = [x(:), y(:)];
402
405
- 
 
 406 
        nCoords = length(x(:));
- 
 
 407
403 
        e0UndistImCoords = undistortPointsFast(e0ImCoords, camStereoParams.CameraParameters1);
 408 
        e0UndistImCoords = undistortPointsFast(e0ImCoords, camStereoParams.CameraParameters1);
404 
        e0NormImCoords = camStereoParams.CameraParameters1.pointsToWorld(eye(3,3), [0, 0, 1], e0UndistImCoords);
 409 
        e0NormImCoords = camStereoParams.CameraParameters1.pointsToWorld(eye(3,3), [0, 0, 1], e0UndistImCoords);
- 
 
 410 
        e0Hom = [e0NormImCoords, ones(nCoords, 1)];
405 
        e1UndistImCoords = undistortPointsFast(e1ImCoords, camStereoParams.CameraParameters2);
 411 
        e1UndistImCoords = undistortPointsFast(e1ImCoords, camStereoParams.CameraParameters2);
406 
        e1NormImCoords = camStereoParams.CameraParameters2.pointsToWorld(eye(3,3), [0, 0, 1], e1UndistImCoords);
 412 
        e1NormImCoords = camStereoParams.CameraParameters2.pointsToWorld(eye(3,3), [0, 0, 1], e1UndistImCoords);
- 
 
 413 
        e1Hom = [e1NormImCoords, ones(nCoords, 1)];
407 
 
 414 
 
- 
 
 415 
        imVals0 = double(frame0(sub2ind(size(frame0), e0ImCoords(:,2), e0ImCoords(:,1))));
- 
 
 416 
        imVals1 = double(frame1(sub2ind(size(frame1), e1ImCoords(:,2), e1ImCoords(:,1))));
- 
 
 417
408 
        x0 = [pI nI(1:2)/nI(3) 70.0 70.0];
 418 
        x0 = [pI nI(1:2)/nI(3) 70.0 70.0];
409
419
410 
        r = circleResiduals(x0);
 420 
%         r = circleResiduals(x0);
411 
        figure;
421 
%         figure;
412 
        subplot(2,2,1);
 422 
%         subplot(2,2,1);
413 
        imagesc(reshape(r(1:length(e0NormImCoords)), 2*margin+1, 2*margin+1), [-50 50]);
 423 
%         imagesc(reshape(r(1:length(e0NormImCoords)), 2*margin+1, 2*margin+1), [-50 50]);
414 
        subplot(2,2,2);
 424 
%         subplot(2,2,2);
415 
        imagesc(reshape(r(length(e0NormImCoords)+1:end), 2*margin+1, 2*margin+1), [-50 50]);
 425 
%         imagesc(reshape(r(length(e0NormImCoords)+1:end), 2*margin+1, 2*margin+1), [-50 50]);
416 
        drawnow;
 426 
%         drawnow;
417 
 
 427 
 
418 
        options = optimset('Algorithm', 'levenberg-marquardt', 'Display', 'iter-detailed', 'OutputFcn', @out, 'MaxIter', 30, 'TolFun', 10^(-5), 'TolX', 0);
 428 
        %options = optimset('Algorithm', 'levenberg-marquardt', 'Display', 'final', 'OutputFcn', @out, 'MaxIter', 30, 'TolFun', 10^(-6), 'TolX', 0);
- 
 
 429 
        options = optimoptions('lsqnonlin', 'Display', 'final');
- 
 
 430
419 
        [x, conf(i), ~] = lsqnonlin(@circleResiduals, x0, [], [], options);
 431 
        [x, conf(i), ~] = lsqnonlin(@circleResiduals, x0, [], [], options);
420
432
421 
        r = circleResiduals(x);
 433 
%         r = circleResiduals(x);
422 
        subplot(2,2,3);
 434 
%         subplot(2,2,3);
423 
        imagesc(reshape(r(1:length(e0NormImCoords)), 2*margin+1, 2*margin+1), [-50 50]);
 435 
%         imagesc(reshape(r(1:length(e0NormImCoords)), 2*margin+1, 2*margin+1), [-50 50]);
424 
        subplot(2,2,4);
 436 
%         subplot(2,2,4);
425 
        imagesc(reshape(r(length(e0NormImCoords)+1:end), 2*margin+1, 2*margin+1), [-50 50]);
 437 
%         imagesc(reshape(r(length(e0NormImCoords)+1:end), 2*margin+1, 2*margin+1), [-50 50]);
426 
        drawnow;
 438 
%         drawnow;
427
439
428 
        E(i,:) = x(1:3);
 440 
        E(i,:) = x(1:3);
429 
 
 441 
 
430 
    end
 442 
    end
431
443
Line 457... Line 469...
457
469
458 
        % norminal radius of markers
 470 
        % norminal radius of markers
459 
        markerRadius = 1.4/2.0; %mm
 471 
        markerRadius = 1.4/2.0; %mm
460
472
461 
        % half-width of ramp
 473 
        % half-width of ramp
462 
        w = 0.3; %mm
 474 
        w = 0.1; %mm
- 
 
 475
- 
 
 476 
        % camera 0
- 
 
 477 
        t = (p0*n0')./(e0Hom*n0');
463 
 
 478 
 
464 
        p0n0 = (p0*n0');
 479 
        dVec = repmat(p0, nCoords, 1) - t.*e0Hom;
465 
        p1n1 = (p1*n1');
 - 
 
466
- 
 
467 
        for k=1:length(e0NormImCoords)
 480 
        d = sqrt(sum(dVec.*dVec, 2));
468 
 
 481 
 
469 
            % dermine homogenous coordinate on the hypothesized plane
 - 
 
470 
            t = p0n0/([e0NormImCoords(k,:), 1]*n0');
 - 
 
471
- 
 
472 
            d = norm(p0 - t*[e0NormImCoords(k,:), 1.0]);
 - 
 
473
- 
 
474 
            imVal = double(frame0(e0ImCoords(k,2), e0ImCoords(k,1)));
 - 
 
475
- 
 
476 
            % "saturated" ramp function for marker disc shape
 482 
        % "saturated" ramp function for marker disc shape
477 
            weight = max(min(1.0, -1.0/(2*w)*(d-markerRadius)+0.5), 0.0);
 483 
        weights = max(min(1.0, -1.0/(2*w)*(d-markerRadius)+0.5), 0.0);
478
- 
 
479 
            r(k) = x(6)*weight - imVal;
 - 
 
480
- 
 
481 
        end
 - 
 
482
- 
 
483 
        for k=1:length(e1NormImCoords)
 - 
 
484 
 
 484 
 
485 
            % dermine z coordinate on the hypothesized plane
 - 
 
486 
            t = p1n1/([e1NormImCoords(k,:), 1]*n1');
 - 
 
487
- 
 
488 
            d = norm(p1 - t*[e1NormImCoords(k,:), 1.0]);
 485 
        r(1:nCoords) = x(6)*weights - imVals0;
489
486
490 
            imVal = double(frame1(e1ImCoords(k,2), e1ImCoords(k,1)));
 - 
 
491
487 
        % camera 1
492 
            % "saturated" ramp function for marker disc shape
 - 
 
493 
            weight = max(min(1.0, -1.0/(2*w)*(d-markerRadius)+0.5), 0.0);
 - 
 
494
- 
 
495 
            r(length(e0NormImCoords)+k) = x(7)*weight - imVal;
 488 
        t = (p1*n1')./(e1Hom*n1');
496 
 
 489 
 
497 
        end
 490 
        dVec = repmat(p1, nCoords, 1) - t.*e1Hom;
498
491 
        d = sqrt(sum(dVec.*dVec, 2));
- 
 
 492 
 
- 
 
 493 
        % "saturated" ramp function for marker disc shape
- 
 
 494 
        weights = max(min(1.0, -1.0/(2*w)*(d-markerRadius)+0.5), 0.0);
- 
 
 495 
 
- 
 
 496 
        r(nCoords+1:end) = x(7)*weights - imVals1;
499
497
500 
%         figure;
498 
%         figure;
501 
%         subplot(1,2,1);
 499 
%         subplot(1,2,1);
502 
%         imagesc(reshape(r(1:length(e0NormImCoords)), 2*margin+1, 2*margin+1), [-50 50]);
 500 
%         imagesc(reshape(r(1:length(e0NormImCoords)), 2*margin+1, 2*margin+1), [-50 50]);
503 
%         subplot(1,2,2);
 501 
%         subplot(1,2,2);
Line 520... Line 518...
520 
    for i=1:size(e, 1)
 518 
    for i=1:size(e, 1)
521 
        sqDists = sum((q - repmat(e(i,:), size(q, 1), 1)).^2, 2);
 519 
        sqDists = sum((q - repmat(e(i,:), size(q, 1), 1)).^2, 2);
522
520
523 
        [sqDistsSorted, sortIdx] = sort(sqDists);
 521 
        [sqDistsSorted, sortIdx] = sort(sqDists);
524
522
- 
 
 523 
        % neighbors are points that project to within 10px
525 
        neighbors = (sqDistsSorted < 4.0^2);
 524 
        neighbors = (sqDistsSorted < 10.0^2);
526
525
527 
        distsSorted = sqrt(sqDistsSorted(neighbors));
 526 
        distsSorted = sqrt(sqDistsSorted(neighbors));
528 
        invDistsSorted = 1.0/distsSorted;
 527 
        invDistsSorted = 1.0./distsSorted;
529 
        sortIdx = sortIdx(neighbors);
 528 
        sortIdx = sortIdx(neighbors);
530
529
531 
        nNeighbors = sum(neighbors);
 530 
        nNeighbors = sum(neighbors);
532
531
533 
        if(nNeighbors >= 2)
 532 
        if(nNeighbors >= 2)