Subversion Repositories seema-scanner

Rev

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

Rev 238 Rev 239
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 = undistortPoints(e0Coords, camParams0);
 77 
    e0Coords = undistortPointsFast(e0Coords, camParams0);
78 
    e1Coords = undistortPoints(e1Coords, camParams1);
 78 
    e1Coords = 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(e0Coords, P0, Q);
82 
    E1 = projectOntoPointCloud(e1Coords, P1, Q);
 82 
    E1 = projectOntoPointCloud(e1Coords, P1, Q);
83 
 
 83 
 
Line 366... Line 366...
366 
function [E, conf] = detectMarkersStereoSubpix(frame0, frame1, initGuesses, camStereoParams, pc)
 366 
function [E, conf] = detectMarkersStereoSubpix(frame0, frame1, initGuesses, camStereoParams, pc)
367 
% Detect a marker in stereo frame set by minimizing the difference to
 367 
% Detect a marker in stereo frame set by minimizing the difference to
368 
% projected images of 3d marker
 368 
% projected images of 3d marker
369
369
370 
    normals = pcnormals(pc, 6);
 370 
    normals = pcnormals(pc, 6);
- 
 
 371
- 
 
 372 
    frame0 = rgb2gray(frame0);
- 
 
 373 
    frame1 = rgb2gray(frame1);
371 
 
 374 
 
372 
    nMarkers = size(initGuesses, 2);
 375 
    nMarkers = size(initGuesses, 2);
373
376
- 
 
 377 
    E = zeros(nMarkers, 3);
- 
 
 378 
    conf = zeros(nMarkers, 1);
- 
 
 379
374 
    for i=1:nMarkers
 380 
    for i=1:nMarkers
375
381
376 
        pI = initGuesses(i,:);
 382 
        pI = initGuesses(i,:);
377
383
378 
        e0 = camStereoParams.CameraParameters1.worldToImage(eye(3,3), zeros(3,1), pI);
 384 
        e0 = camStereoParams.CameraParameters1.worldToImage(eye(3,3), zeros(3,1), pI);
Line 382... Line 388...
382 
        e0Center = round(e0);
 388 
        e0Center = round(e0);
383 
        e1Center = round(e1);
 389 
        e1Center = round(e1);
384
390
385 
        % find initial normal
 391 
        % find initial normal
386 
        idx = pc.findNearestNeighbors(pI, 1);
 392 
        idx = pc.findNearestNeighbors(pI, 1);
387 
        nI = normals(idx, :);
 393 
        nI = double(normals(idx, :));
388
394
389 
        margin = 10;
 395 
        margin = 25;
390
396
391 
        [x,y] = meshgrid(e0Center(1)-margin:e0Center(1)+margin, e0Center(2)-margin:e0Center(2)+margin);
 397 
        [x,y] = meshgrid(e0Center(1)-margin:e0Center(1)+margin, e0Center(2)-margin:e0Center(2)+margin);
392 
        e0ImCoords = [x(:), y(:)];
 398 
        e0ImCoords = [x(:), y(:)];
393
399
394 
        [x,y] = meshgrid(e1Center(1)-margin:e1Center(1)+margin, e1Center(2)-margin:e1Center(2)+margin);
 400 
        [x,y] = meshgrid(e1Center(1)-margin:e1Center(1)+margin, e1Center(2)-margin:e1Center(2)+margin);
395 
        e1ImCoords = [x(:), y(:)];
 401 
        e1ImCoords = [x(:), y(:)];
396
402
397 
        e0UndistImCoords = undistortPoints(e0ImCoords, camStereoParams.CameraParameters1);
 403 
        e0UndistImCoords = undistortPointsFast(e0ImCoords, camStereoParams.CameraParameters1);
398 
        e0NormImCoords = camStereoParams.CameraParameters1.pointsToWorld(eye(3,3), zeros(3,1), e0UndistImCoords);
 404 
        e0NormImCoords = camStereoParams.CameraParameters1.pointsToWorld(eye(3,3), [0, 0, 1], e0UndistImCoords);
399 
        e1UndistImCoords = undistortPoints(e1ImCoords, camStereoParams.CameraParameters2);
 405 
        e1UndistImCoords = undistortPointsFast(e1ImCoords, camStereoParams.CameraParameters2);
400 
        e1NormImCoords = camStereoParams.CameraParameters2.pointsToWorld(camStereoParams.RotationOfCamera2, camStereoParams.TranslationOfCamera2, e1UndistImCoords);
 406 
        e1NormImCoords = camStereoParams.CameraParameters2.pointsToWorld(eye(3,3), [0, 0, 1], e1UndistImCoords);
401 
 
 407 
 
402 
        x0 = [pI; nI(1:2)];
 408 
        x0 = [pI nI(1:2)/nI(3) 70.0 70.0];
403
409
- 
 
 410 
        options = optimset('Algorithm', 'levenberg-marquardt', 'Display', 'iter-detailed', 'OutputFcn', @out, 'MaxIter', 30, 'TolFun', 10^(-5), 'TolX', 0);
- 
 
 411 
        [x, conf(i), ~] = lsqnonlin(@circleResiduals, x0, [], [], options);
404
412
- 
 
 413 
        display(x);
- 
 
 414 
        E(i,:) = x(1:3);
- 
 
 415 
 
- 
 
 416 
    end
- 
 
 417
- 
 
 418 
    function stop = out(x, optimValues, state)
405
419
- 
 
 420 
%         r = optimValues.residual;
- 
 
 421 
%
- 
 
 422 
%         figure;
- 
 
 423 
%         subplot(1,2,1);
- 
 
 424 
%         imagesc(reshape(r(1:length(e0NormImCoords)), 2*margin+1, 2*margin+1), [-50 50]);
- 
 
 425 
%         subplot(1,2,2);
- 
 
 426 
%         imagesc(reshape(r(length(e0NormImCoords)+1:end), 2*margin+1, 2*margin+1), [-50 50]);
- 
 
 427 
%         drawnow;
- 
 
 428 
%
- 
 
 429 
%         display(x);
406
430
- 
 
 431 
        stop = false;
407 
    end
 432 
    end
408
433 
 
409 
    function r = circleResiduals(x)
 434 
    function r = circleResiduals(x)
410
435
411 
        point = x(1:3);
 436 
        p0 = x(1:3);
- 
 
 437 
        p1 = x(1:3) * camStereoParams.RotationOfCamera2 + camStereoParams.TranslationOfCamera2;
412 
        normal = [x(4:5); sqrt(1-x(4)^2-x(5)^2)];
 438 
        n0 = [x(4:5) 1.0];
- 
 
 439 
        n0 = 1.0/norm(n0) * n0;
- 
 
 440 
        n1 = n0 * camStereoParams.RotationOfCamera2;
- 
 
 441
- 
 
 442 
        r = zeros(length(e0NormImCoords) + length(e1NormImCoords), 1);
- 
 
 443
- 
 
 444 
        % norminal radius of markers
- 
 
 445 
        markerRadius = 1.4/2.0; %mm
413
446
414 
        r = zeros(length(length(e0NormImCoords)) + length(e1NormImCoords), 1);
 447 
        % half-width of ramp
- 
 
 448 
        w = 0.3; %mm
- 
 
 449 
 
- 
 
 450 
        p0n0 = (p0*n0');
- 
 
 451 
        p1n1 = (p1*n1');
415
452
416 
        for i=1:length(e0NormImCoords)
 453 
        for k=1:length(e0NormImCoords)
- 
 
 454 
 
- 
 
 455 
            % dermine homogenous coordinate on the hypothesized plane
- 
 
 456 
            t = p0n0/([e0NormImCoords(k,:), 1]*n0');
- 
 
 457
- 
 
 458 
            d = norm(p0 - t*[e0NormImCoords(k,:), 1.0]);
417
459
- 
 
 460 
            imVal = double(frame0(e0ImCoords(k,2), e0ImCoords(k,1)));
- 
 
 461
- 
 
 462 
            % "saturated" ramp function for marker disc shape
- 
 
 463 
            weight = max(min(1.0, -1.0/(2*w)*(d-markerRadius)+0.5), 0.0);
- 
 
 464
- 
 
 465 
            r(k) = x(6)*weight - imVal;
- 
 
 466
- 
 
 467 
        end
- 
 
 468
- 
 
 469 
        for k=1:length(e1NormImCoords)
- 
 
 470 
 
418 
            % dermine z coordinate on the hypothesized plane
 471 
            % dermine z coordinate on the hypothesized plane
419 
            z =
472 
            t = p1n1/([e1NormImCoords(k,:), 1]*n1');
420
473
- 
 
 474 
            d = norm(p1 - t*[e1NormImCoords(k,:), 1.0]);
- 
 
 475
- 
 
 476 
            imVal = double(frame1(e1ImCoords(k,2), e1ImCoords(k,1)));
- 
 
 477
- 
 
 478 
            % "saturated" ramp function for marker disc shape
- 
 
 479 
            weight = max(min(1.0, -1.0/(2*w)*(d-markerRadius)+0.5), 0.0);
- 
 
 480
- 
 
 481 
            r(length(e0NormImCoords)+k) = x(7)*weight - imVal;
- 
 
 482 
 
421 
        end
 483 
        end
422
484
423
485
- 
 
 486 
%         figure;
- 
 
 487 
%         subplot(1,2,1);
- 
 
 488 
%         imagesc(reshape(r(1:length(e0NormImCoords)), 2*margin+1, 2*margin+1), [-50 50]);
- 
 
 489 
%         subplot(1,2,2);
- 
 
 490 
%         imagesc(reshape(r(length(e0NormImCoords)+1:end), 2*margin+1, 2*margin+1), [-50 50]);
- 
 
 491 
%         drawnow;
- 
 
 492
424 
    end
 493 
    end
425
494
426 
end
 495 
end
427 
 
 496 
 
428 
function E = projectOntoPointCloud(e, P, pointCloud)
 497 
function E = projectOntoPointCloud(e, P, pointCloud)