gusucode.com > vision工具箱matlab源码程序 > vision/pcdenoise.m
function [ptCloudOut, inlierIndices, outlierIndices] = pcdenoise(ptCloudIn, varargin) %PCDENOISE Remove noise from a 3-D point cloud. % ptCloudOut = PCDENOISE(ptCloudIn) returns a filtered point cloud that % removes outliers. ptCloudIn and ptCloudOut are point cloud objects. A % point is considered to be an outlier if the average distance to its K % nearest neighbors is above a threshold. By default, the threshold is % chosen to be one standard deviation from the mean of average distance % to neighbors of all points. % % [..., inlierIndices, outlierIndices] = PCDENOISE(...) additionally % returns the linear indices to the points that are identified as inliers % and outliers. % % [...] = PCDENOISE(..., Name, Value) specifies additional name-value % pairs described below: % % 'NumNeighbors' A positive integer specifying the number of nearest % neighbors used to estimate the mean distance. A % small value tend to be sensitive to the noise, while % a large value is more computationally expensive. % % Default: 4 % % 'Threshold' A scalar specifying the number of standard deviation % away from the mean distance. Decrease this value to % remove more points. % % Default: 1.0 % % Class Support % ------------- % ptCloudIn and ptCloutOut must be pointCloud object. inlierIndices and % outlierIndices are uint32. % % Example: Remove outliers from a noisy point cloud % ------------------------------------------------- % % Create a plane point cloud % gv = 0:0.01:1; % [X,Y] = meshgrid(gv,gv); % ptCloud = pointCloud([X(:),Y(:),0.5*ones(numel(X),1)]); % % figure % pcshow(ptCloud); % title('Original Data'); % % % Add uniformly distributed random noise % noise = rand(500, 3); % ptCloudA = pointCloud([ptCloud.Location; noise]); % % figure % pcshow(ptCloudA); % title('Noisy Data'); % % ptCloudB = pcdenoise(ptCloudA); % % figure; % pcshow(ptCloudB); % title('Denoised Data'); % % See also pointCloud, pcshow, pctransform, pcdownsample, pcmerge % Copyright 2014 The MathWorks, Inc. % % References % ---------- % R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz. Towards 3D % Point Cloud Based Object Maps for Household Environments Robotics and % Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008 narginchk(1, 5); % Parse the inputs [numNeighbors, threshold] = validateAndParseInputs(ptCloudIn, varargin{:}); if ~ismatrix(ptCloudIn.Location) points = reshape(ptCloudIn.Location, [], 3); else points = ptCloudIn.Location; end % Compute the mean distance to neighbors for every point, and exclude the % query point itself [~, dists, valids] = multiQueryKNNSearchImpl(ptCloudIn, points, numNeighbors+1); % This multi-query KNN search uses exact search so every query should % return the same number of neighbors if the query is a valid point actualNums = double(max(valids(:))-1); meanDist = sum(dists(2:actualNums, :),1)/actualNums; isValidPoints = isfinite(points); isValidPoints = sum(isValidPoints,2)==3; meanDist(~isValidPoints) = NaN; % Compute the threshold finiteMeanDist = meanDist(isfinite(meanDist)); meanD = mean(finiteMeanDist); stdD = std(finiteMeanDist); distThreshold = meanD+threshold*stdD; % Select the inliers tf = meanDist <= distThreshold; inlierIndices = uint32(find(tf)); [loc, color, nv] = subsetImpl(ptCloudIn, inlierIndices); ptCloudOut = pointCloud(loc, 'Color', color, 'Normal', nv); if nargout == 3 outlierIndices = uint32(find(~tf)); end end function [numNeighbors, threshold] = validateAndParseInputs(ptCloudIn, varargin) % Validate the first argument if ~isa(ptCloudIn, 'pointCloud') error(message('vision:pointcloud:notPointCloudObject', 'ptCloudIn')); end % Validate and parse optional inputs parser = inputParser; parser.CaseSensitive = false; parser.FunctionName = mfilename; parser.addParameter('NumNeighbors', 4, @(x)validateattributes(x,{'single', 'double'}, {'scalar','integer','positive'})); parser.addParameter('Threshold', 1.0, @(x)validateattributes(x,{'single', 'double'}, {'scalar','real','finite'})); parser.parse(varargin{:}); numNeighbors = parser.Results.NumNeighbors; threshold = parser.Results.Threshold; end