gusucode.com > vision工具箱matlab源码程序 > vision/pcfitsphere.m
function [model, inlierIndices, outlierIndices, rmse] = pcfitsphere(varargin) %PCFITSPHERE Fit sphere to a 3-D point cloud. % model = PCFITSPHERE(ptCloud, maxDistance) fits a sphere to the point % cloud, ptCloud. The sphere is described by a sphereModel object. % maxDistance is a maximum allowed distance from an inlier point to the % sphere. This function uses the M-estimator SAmple Consensus (MSAC) % algorithm to find the sphere. % % [..., inlierIndices, outlierIndices] = PCFITSPHERE(...) additionally % returns linear indices to the inlier and outlier points in ptCloud. % % [..., rmse] = PCFITSPHERE(...) additionally returns root mean square % error of the distance of inlier points to the model. % % [...] = PCFITSPHERE(..., 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 pointCloud object. % % Example: Detect sphere 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-sphere distance (1cm) for sphere fitting % maxDistance = 0.01; % % % Set the roi to constrain the search % roi = [-inf, 0.5, 0.2, 0.4, 0.1, inf]; % sampleIndices = findPointsInROI(ptCloud, roi); % % % Detect the globe and extract it from the point cloud % [model, inlierIndices] = pcfitsphere(ptCloud, maxDistance,... % 'SampleIndices', sampleIndices); % globe = select(ptCloud, inlierIndices); % % % Plot the globe % hold on % plot(model) % % figure % pcshow(globe) % title('Globe Point Cloud') % % See also pointCloud, pointCloud>findPointsInROI, sphereModel, pcfitplane, % 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] = ... vision.internal.ransac.validateAndParseRansacInputs(mfilename, false,... varargin{:}); % Use four points to fit a sphere sampleSize = 4; % Initialization [statusCode, status, pc, validPtCloudIndices] = ... vision.internal.ransac.initializeRansacModel(ptCloud, sampleIndices, ... sampleSize); ransacParams.sampleSize = sampleSize; % Compute the geometric model parameter with MSAC if status == statusCode.NoError ransacFuncs.fitFunc = @fitSphere; ransacFuncs.evalFunc = @evalSphere; ransacFuncs.checkFunc = @checkSphere; [isFound, modelParams] = vision.internal.ransac.msac(pc.Location, ... ransacParams, ransacFuncs); if ~isFound status = statusCode.NotEnoughInliers; end end if status == statusCode.NoError % Construct the sphere object model = sphereModel(modelParams); else model = sphereModel.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 = evalSphere(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 %========================================================================== % Sphere equation: (x-a)^2 + (y-b)^2 + (z-c)^2 = d^2; %========================================================================== function model = fitSphere(points) X = [points, ones(size(points,1),1)]; m11 = det(X); if abs(m11)<=eps(class(points)) model = []; return; end X(:,1) = points(:,1).^2+points(:,2).^2+points(:,3).^2; m12 = det(X); X(:,2) = X(:,1); X(:,1) = points(:,1); m13 = det(X); X(:,3) = X(:,2); X(:,2) = points(:,2); m14 = det(X); X(:,1) = X(:,3); X(:,2:4) = points; m15 = det(X); a = 0.5*m12/m11; b = 0.5*m13/m11; c = 0.5*m14/m11; d = sqrt(a^2+b^2+c^2-m15/m11); model = [a, b, c, d]; %========================================================================== % Calculate the distance from the point to the sphere. % D = abs(sqrt((a-x)^2 + (b-y)^2 + (c-z)^2)-d) %========================================================================== function dis = evalSphere(model, points) dis = abs(sqrt((points(:,1)-model(1)).^2 + (points(:,2)-model(2)).^2 + ... (points(:,3)-model(3)).^2) - model(4)); %========================================================================== % Validate the sphere coefficients %========================================================================== function isValid = checkSphere(model) isValid = (numel(model) == 4 & all(isfinite(model)));