使用经标定的相机测量平面目标

images = imageDatastore(fullfile(toolboxdir("vision"),"visiondata",...
    "calibration","slr"));
files = images.Files;

% Display one of the calibration images
I = imread(files{1});
figure
imshow(I)
title("One of the Calibration Images")

% Detect the checkerboard corners in the images.
[imagePoints, patternDims] = detectCheckerboardPoints(files);

% Generate the world coordinates of the checkerboard corners in the
% pattern-centric coordinate system, with the upper-left corner at (0,0).
squareSize = 29; % in millimeters
worldPoints = patternWorldPoints("checkerboard", patternDims, squareSize);

% Calibrate the camera.
imageSize = size(I,1:2);
cameraParams = estimateCameraParameters(imagePoints, worldPoints, ImageSize=imageSize);

% Evaluate calibration accuracy.
figure
showReprojectionErrors(cameraParams)
title("Reprojection Errors")

imOrig = imread(fullfile(matlabroot, "toolbox", "vision", "visiondata", ...
        "calibration", "slr", "image9.jpg"));
figure
imshow(imOrig)
title("Input Image")

% Since the lens introduced little distortion, use 'full' output view to illustrate that
% the image was undistorted. If we used the default 'same' option, it would be difficult
% to notice any difference when compared to the original image. Notice the small black borders.
[im, newIntrinsics] = undistortImage(imOrig, cameraParams, OutputView="full");
figure
imshow(im)
title("Undistorted Image")

% Convert the image to the HSV color space.
imHSV = rgb2hsv(im);

% Get the saturation channel.
saturation = imHSV(:, :, 2);

% Threshold the image
t = graythresh(saturation);
imCoin = (saturation > t);

figure
imshow(imCoin)
title("Segmented Coins")

% Find connected components.
blobAnalysis = vision.BlobAnalysis(AreaOutputPort=true,...
    CentroidOutputPort=false,...
    BoundingBoxOutputPort=true,...
    MinimumBlobArea=200, ExcludeBorderBlobs=true);
[areas, boxes] = step(blobAnalysis, imCoin);

% Sort connected components in descending order by area
[~, idx] = sort(areas, "Descend");

% Get the two largest components.
boxes = double(boxes(idx(1:2), :));

% Reduce the size of the image for display.
scale = 0.25;
imDetectedCoins = imresize(im, scale);

% Insert labels for the coins.
imDetectedCoins = insertObjectAnnotation(imDetectedCoins, "rectangle", ...
    scale*boxes, "penny");
figure
imshow(imDetectedCoins)
title("Detected Coins")

% Detect the checkerboard.
[imagePoints, patternDims] = detectCheckerboardPoints(im);

% Extract camera intrinsics.
camIntrinsics = cameraParams.Intrinsics;

% Adjust the imagePoints so that they are expressed in the coordinate system
% used in the original image, before it was undistorted.  This adjustment
% makes it compatible with the cameraParameters object computed for the original image.
newOrigin = camIntrinsics.PrincipalPoint - newIntrinsics.PrincipalPoint;
imagePoints = imagePoints + newOrigin; % adds newOrigin to every row of imagePoints

% Compute extrinsic parameters of the camera.
camExtrinsics = estimateExtrinsics(imagePoints, worldPoints, camIntrinsics);

% Adjust upper left corners of bounding boxes for coordinate system shift 
% caused by undistortImage with output view of 'full'. This would not be
% needed if the output was 'same'. The adjustment makes the points compatible
% with the cameraParameters of the original image.
boxes = boxes + [newOrigin, 0, 0]; % zero padding is added for width and height

% Get the top-left and the top-right corners.
box1 = double(boxes(1, :));
imagePoints1 = [box1(1:2); ...
                box1(1) + box1(3), box1(2)];

% Get the world coordinates of the corners            
worldPoints1 = img2world2d(imagePoints1, camExtrinsics, camIntrinsics);

% Compute the diameter of the coin in millimeters.
d = worldPoints1(2, :) - worldPoints1(1, :);
diameterInMillimeters = hypot(d(1), d(2));
fprintf("Measured diameter of one penny = %0.2f mm\n", diameterInMillimeters);

% Get the top-left and the top-right corners.
box2 = double(boxes(2, :));
imagePoints2 = [box2(1:2); ...
                box2(1) + box2(3), box2(2)];

% Apply the inverse transformation from image to world            
worldPoints2 = img2world2d(imagePoints2, camExtrinsics, camIntrinsics);

% Compute the diameter of the coin in millimeters.
d = worldPoints2(2, :) - worldPoints2(1, :);
diameterInMillimeters = hypot(d(1), d(2));
fprintf("Measured diameter of the other penny = %0.2f mm\n", diameterInMillimeters);

% Compute the center of the first coin in the image.
center1_image = box1(1:2) + box1(3:4)/2;

% Convert to world coordinates.
center1_world  = img2world2d(center1_image, camExtrinsics, camIntrinsics);

% Remember to add the 0 z-coordinate.
center1_world = [center1_world 0];

% Compute the distance to the camera.
cameraPose = extr2pose(camExtrinsics);
cameraLocation = cameraPose.Translation;
distanceToCamera = norm(center1_world - cameraLocation);
fprintf("Distance from the camera to the first penny = %0.2f mm\n", ...
    distanceToCamera);