Subversion Repositories seema-scanner

Rev

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

Rev 237 Rev 238
Line 106... Line 106...
106 
    imshow(frame1);
 106 
    imshow(frame1);
107 
    hold('on');
 107 
    hold('on');
108 
    plot(e1Coords(matchedPairs(:, 2),1), e1Coords(matchedPairs(:, 2),2), 'rx', 'MarkerSize', 15);
 108 
    plot(e1Coords(matchedPairs(:, 2),1), e1Coords(matchedPairs(:, 2),2), 'rx', 'MarkerSize', 15);
109 
    drawnow;
 109 
    drawnow;
110
110
111 
    % triangulate marker centers (lens correction has been performed)
 111 
%     % triangulate marker centers (lens correction has been performed)
112 
    [E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);
 112 
%     [E{i}, e] = triangulate(e0Coords(matchedPairs(:, 1),:), e1Coords(matchedPairs(:, 2),:), camStereoParams);
113 
    E{i} = E{i}(e<3.0, :);
 113 
%     E{i} = E{i}(e<3.0, :);
- 
 
 114 
%     display(e);
- 
 
 115
- 
 
 116 
    [E{i}, e] = detectMarkersStereoSubpix(frame0, frame1, E0(matchedPairs(:, 1), :), camStereoParams, pc);
114 
    display(e);
 117 
    display(e);
115
118
116 
    % write point cloud with marker (debugging)
 119 
    % write point cloud with marker (debugging)
117 
    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)]);
118 
    pcwrite(pcDebug, 'pcDebug.ply');
 121 
    pcwrite(pcDebug, 'pcDebug.ply');
Line 358... Line 361...
358
361
359 
    e = e(1:nMarkersFound, :);
 362 
    e = e(1:nMarkersFound, :);
360 
    conf = conf(1:nMarkersFound);
 363 
    conf = conf(1:nMarkersFound);
361 
end
 364 
end
362 
 
 365 
 
- 
 
 366 
function [E, conf] = detectMarkersStereoSubpix(frame0, frame1, initGuesses, camStereoParams, pc)
- 
 
 367 
% Detect a marker in stereo frame set by minimizing the difference to
- 
 
 368 
% projected images of 3d marker
- 
 
 369
- 
 
 370 
    normals = pcnormals(pc, 6);
- 
 
 371 
 
- 
 
 372 
    nMarkers = size(initGuesses, 2);
- 
 
 373
- 
 
 374 
    for i=1:nMarkers
- 
 
 375
- 
 
 376 
        pI = initGuesses(i,:);
- 
 
 377
- 
 
 378 
        e0 = camStereoParams.CameraParameters1.worldToImage(eye(3,3), zeros(3,1), pI);
- 
 
 379 
        e1 = camStereoParams.CameraParameters2.worldToImage(camStereoParams.RotationOfCamera2, camStereoParams.TranslationOfCamera2, pI);
- 
 
 380
- 
 
 381 
        % center of support region
- 
 
 382 
        e0Center = round(e0);
- 
 
 383 
        e1Center = round(e1);
- 
 
 384
- 
 
 385 
        % find initial normal
- 
 
 386 
        idx = pc.findNearestNeighbors(pI, 1);
- 
 
 387 
        nI = normals(idx, :);
- 
 
 388
- 
 
 389 
        margin = 10;
- 
 
 390
- 
 
 391 
        [x,y] = meshgrid(e0Center(1)-margin:e0Center(1)+margin, e0Center(2)-margin:e0Center(2)+margin);
- 
 
 392 
        e0ImCoords = [x(:), y(:)];
- 
 
 393
- 
 
 394 
        [x,y] = meshgrid(e1Center(1)-margin:e1Center(1)+margin, e1Center(2)-margin:e1Center(2)+margin);
- 
 
 395 
        e1ImCoords = [x(:), y(:)];
- 
 
 396
- 
 
 397 
        e0UndistImCoords = undistortPoints(e0ImCoords, camStereoParams.CameraParameters1);
- 
 
 398 
        e0NormImCoords = camStereoParams.CameraParameters1.pointsToWorld(eye(3,3), zeros(3,1), e0UndistImCoords);
- 
 
 399 
        e1UndistImCoords = undistortPoints(e1ImCoords, camStereoParams.CameraParameters2);
- 
 
 400 
        e1NormImCoords = camStereoParams.CameraParameters2.pointsToWorld(camStereoParams.RotationOfCamera2, camStereoParams.TranslationOfCamera2, e1UndistImCoords);
- 
 
 401 
 
- 
 
 402 
        x0 = [pI; nI(1:2)];
- 
 
 403
- 
 
 404
- 
 
 405
- 
 
 406
- 
 
 407 
    end
- 
 
 408
- 
 
 409 
    function r = circleResiduals(x)
- 
 
 410
- 
 
 411 
        point = x(1:3);
- 
 
 412 
        normal = [x(4:5); sqrt(1-x(4)^2-x(5)^2)];
- 
 
 413
- 
 
 414 
        r = zeros(length(length(e0NormImCoords)) + length(e1NormImCoords), 1);
- 
 
 415
- 
 
 416 
        for i=1:length(e0NormImCoords)
- 
 
 417
- 
 
 418 
            % dermine z coordinate on the hypothesized plane
- 
 
 419 
            z =
- 
 
 420
- 
 
 421 
        end
- 
 
 422
- 
 
 423
- 
 
 424 
    end
- 
 
 425
- 
 
 426 
end
- 
 
 427 
 
363 
function E = projectOntoPointCloud(e, P, pointCloud)
 428 
function E = projectOntoPointCloud(e, P, pointCloud)
364 
% Project 2d point coordinates onto pointCloud to find corresponding 3d
 429 
% Project 2d point coordinates onto pointCloud to find corresponding 3d
365 
% point coordinates.
 430 
% point coordinates.
366 
 
 431 
 
367 
    q = [pointCloud ones(size(pointCloud,1),1)]*P;
 432 
    q = [pointCloud ones(size(pointCloud,1),1)]*P;