gusucode.com > vision工具箱matlab源码程序 > vision/pcfitplane.m
function [model, inlierIndices, outlierIndices, rmse] = pcfitplane(varargin)
%PCFITPLANE Fit plane to a 3-D point cloud.
% model = PCFITPLANE(ptCloud, maxDistance) fits a plane to the point
% cloud, ptCloud. The plane is described by a planeModel object.
% maxDistance is a maximum allowed distance from an inlier point to the
% plane. This function uses the M-estimator SAmple Consensus (MSAC)
% algorithm to find the plane.
%
% model = PCFITPLANE(..., referenceVector) fits a plane to the point
% cloud with additional orientation constraints. referenceVector is a
% 1-by-3 vector used as a reference orientation.
%
% model = PCFITPLANE(..., referenceVector, maxAngularDistance) fits a
% plane to the point cloud with additional orientation constraints.
% maxAngularDistance specifies the maximum allowed absolute angular
% distance in degrees between the normal vector of fitted plane and the
% reference orientation. If it is not specified, the default is 5
% degrees.
%
% [..., inlierIndices, outlierIndices] = PCFITPLANE(...) additionally
% returns linear indices to the inlier and outlier points in ptCloud.
%
% [..., rmse] = PCFITPLANE(...) additionally returns root mean square
% error of the distance of inlier points to the model.
%
% [...] = PCFITPLANE(..., Name,Value) specifies additional name-value
% pair arguments described below:
%
% 'SampleIndices' A vector specifying the linear indices of points
% to be sampled in the input point cloud. The
% indices, for example, can be generated by
% findPointsInROI method of pointCloud object.
% By default, the entire point cloud is processed.
%
% Default: []
%
% 'MaxNumTrials' A positive integer scalar specifying the maximum
% number of random trials for finding the inliers.
% Increasing this value will improve the robustness
% of the output at the expense of additional
% computation.
%
% Default: 1000
%
% 'Confidence' A numeric scalar, C, 0 < C < 100, specifying the
% desired confidence (in percentage) for finding
% the maximum number of inliers. Increasing this
% value will improve the robustness of the output
% at the expense of additional computation.
%
% Default: 99
%
% Class Support
% -------------
% ptCloud must be a pointCloud object.
%
% Example: Detect multiple planes from point cloud
% ------------------------------------------------
% load('object3d.mat')
%
% figure
% pcshow(ptCloud)
% xlabel('X(m)')
% ylabel('Y(m)')
% zlabel('Z(m)')
% title('Original Point Cloud')
%
% % Set the maximum point-to-plane distance (2cm) for plane fitting
% maxDistance = 0.02;
% % Set the normal vector of a plane
% referenceVector = [0, 0, 1];
% % Set the maximum angular distance (5 degrees)
% maxAngularDistance = 5;
%
% % Detect the table and extract it from the point cloud
% [model1, inlierIndices, outlierIndices] = pcfitplane(ptCloud, ...
% maxDistance, referenceVector, maxAngularDistance);
% plane1 = select(ptCloud, inlierIndices);
% remainPtCloud = select(ptCloud, outlierIndices);
%
% % Set the roi to constrain the search for the left wall
% roi = [-inf, inf, 0.4, inf, -inf, inf];
% sampleIndices = findPointsInROI(remainPtCloud, roi);
%
% % Detect the left wall and extract it from the remaining point cloud
% [model2, inlierIndices, outlierIndices] = pcfitplane(remainPtCloud, ...
% maxDistance, 'SampleIndices', sampleIndices);
% plane2 = select(remainPtCloud, inlierIndices);
% remainPtCloud = select(remainPtCloud, outlierIndices);
%
% % Plot the two planes and the remaining points
% figure
% pcshow(plane1)
% title('First Plane')
%
% figure
% pcshow(plane2)
% title('Second Plane')
%
% figure
% pcshow(remainPtCloud)
% title('Remaining Point Cloud')
%
% See also pointCloud, pointCloud>findPointsInROI, planeModel, pcfitsphere,
% pcfitcylinder, pcshow
% Copyright 2015 The MathWorks, Inc.
%
% References:
% ----------
% P. H. S. Torr and A. Zisserman, "MLESAC: A New Robust Estimator with
% Application to Estimating Image Geometry," Computer Vision and Image
% Understanding, 2000.
% Parse input arguments
[ptCloud, ransacParams, sampleIndices, ...
referenceVector, maxAngularDistance] = ...
vision.internal.ransac.validateAndParseRansacInputs(mfilename, true, varargin{:});
% Use three points to fit a plane
sampleSize = 3;
ransacParams.sampleSize = sampleSize;
% Initialization
[statusCode, status, pc, validPtCloudIndices] = ...
vision.internal.ransac.initializeRansacModel(ptCloud, sampleIndices, sampleSize);
% Compute the geometric model parameter with MSAC
if status == statusCode.NoError
ransacFuncs.fitFunc = @fitPlane;
ransacFuncs.evalFunc = @evalPlane;
if isempty(referenceVector)
ransacFuncs.checkFunc = @checkPlane;
[isFound, modelParams] = vision.internal.ransac.msac(pc.Location, ...
ransacParams, ransacFuncs);
else
% Fit the plane with orientation constraint
ransacFuncs.checkFunc = @checkPerpendicularPlane;
denorm = sqrt(dot(referenceVector,referenceVector));
normAxis = referenceVector ./ denorm;
[isFound, modelParams] = vision.internal.ransac.msac(pc.Location, ...
ransacParams, ransacFuncs, normAxis, maxAngularDistance);
if isFound
% Adjust the plane normal vector so that its direction matches the
% referenceVector. This makes the absolute angular distance always
% smaller than 90 degrees.
a = min(1, max(-1, dot(normAxis, modelParams(1:3))));
angle = abs(acos(a));
if angle > pi/2
modelParams = -modelParams;
end
end
end
if ~isFound
status = statusCode.NotEnoughInliers;
end
end
if status == statusCode.NoError
% Construct the plane object
model = planeModel(modelParams);
else
model = planeModel.empty;
end
% Report runtime error
vision.internal.ransac.checkRansacRuntimeStatus(statusCode, status);
% Extract inliers
needInlierIndices = (nargout > 1);
needOutlierIndices = (nargout > 2);
needRMSE = (nargout > 3);
if needInlierIndices
if status == statusCode.NoError
% Re-evaluate the best model
if ~isempty(sampleIndices)
[pc, validPtCloudIndices] = removeInvalidPoints(ptCloud);
end
distances = evalPlane(modelParams, pc.Location);
inlierIndices = validPtCloudIndices(distances < ransacParams.maxDistance);
else
inlierIndices = [];
end
end
% Extract outliers
if needOutlierIndices
if status == statusCode.NoError
flag = true(ptCloud.Count, 1);
flag(inlierIndices) = false;
outlierIndices = find(flag);
else
outlierIndices = [];
end
end
% Report RMSE
if needRMSE
if status == statusCode.NoError
rmse = mean(distances(distances < ransacParams.maxDistance));
else
rmse = [];
end
end
%==========================================================================
% Plane equation: ax + by + cz + d = 0;
%==========================================================================
function model = fitPlane(points, varargin)
a = points(2, :) - points(1, :);
b = points(3, :) - points(1, :);
% Cross product
normal = [a(2).*b(3)-a(3).*b(2), ...
a(3).*b(1)-a(1).*b(3), ...
a(1).*b(2)-a(2).*b(1)];
denom = sum(normal.^2);
if denom < eps(class(points))
model = [];
else
normal = normal / sqrt(denom);
d = -points(1,:) * normal';
model = [normal, d];
end
%==========================================================================
% Calculate the distance from the point to the plane.
% D = (a*x + b*y + c*z + d)/sqrt(a^2+b^2+c^2). Denominator is 1 because
% the normal is a unit vector here.
%==========================================================================
function dis = evalPlane(model, points, varargin)
dis = abs(points * model(1:3)' + model(4));
%==========================================================================
% Validate the plane coefficients
%==========================================================================
function isValid = checkPlane(model)
isValid = (numel(model) == 4 & all(isfinite(model)));
%==========================================================================
% Validate the plane coefficients with orientation constraints
%==========================================================================
function isValid = checkPerpendicularPlane(model, normAxis, threshold)
isValid = checkPlane(model);
if isValid
a = min(1, max(-1, normAxis*model(1:3)'));
angle = abs(acos(a));
angle = min(angle, pi-angle);
isValid = (angle < threshold);
end