gusucode.com > vision工具箱matlab源码程序 > vision/pcmerge.m

    function ptCloudOut = pcmerge(ptCloudA, ptCloudB, gridStep)
%PCMERGE Merge two 3-D point clouds.
%   ptCloudOut = PCMERGE(ptCloudA, ptCloudB, gridStep) returns a
%   merged point cloud using box grid filter. ptCloudA, ptCloudB and
%   ptCloudOut are pointCloud objects. 'gridStep' specifies the size of a
%   3-D box. Points within the same box in the overlapped region are merged
%   to a single point in the output. Their color and normal properties are
%   averaged accordingly.
%
%   Notes 
%   ----- 
%   - Points with NaN or Inf coordinates will be filtered out. 
%
%   - If the two point clouds do not have the same set of properties
%     filled, such as Color or Normal, these properties will be cleared in
%     the returned point cloud. For example, if ptCloudA has color but
%     ptCloudB does not, then ptCloudOut will not contain color.
%
%   Class Support 
%   ------------- 
%   ptCloudA, ptCloudB and ptCloutOut must be pointCloud object. gridStep
%   must be single or double.
% 
%   Example: Merge two identical point clouds with box grid filter 
%   -------------------------------------------------------------- 
%   ptCloudA = pointCloud(100*rand(1000,3));
%   ptCloudB = copy(ptCloudA);
%
%   ptCloud = pcmerge(ptCloudA, ptCloudB, 1);
%   pcshow(ptCloud);
%
% See also pointCloud, pcshow, pctransform, pcdownsample
 
%  Copyright 2014 The MathWorks, Inc.

% Validate input arguments
if ~isa(ptCloudA, 'pointCloud')
    error(message('vision:pointcloud:notPointCloudObject', 'ptCloudA'));
end
if ~isa(ptCloudB, 'pointCloud')
    error(message('vision:pointcloud:notPointCloudObject', 'ptCloudB'));
end
validateattributes(gridStep, {'single','double'}, {'scalar', 'real', 'positive'}, mfilename, 'gridStep');

% Remove invalid points to determine a bounded volume
pcA = removeInvalidPoints(ptCloudA);
pcB = removeInvalidPoints(ptCloudB);

if (isempty(pcA.Location) && isempty(pcB.Location))
    ptCloudOut = pcA;
elseif (isempty(pcA.Location) && ~isempty(pcB.Location))
    ptCloudOut = pcB;
elseif (isempty(pcB.Location) && ~isempty(pcA.Location))
    ptCloudOut = pcA;    
else
    % Combine two inputs
    points = vertcat(pcA.Location, pcB.Location);
    color = vertcat(pcA.Color, pcB.Color);
    if numel(color) ~= numel(points)
        color = uint8.empty;
    end
    normal = vertcat(pcA.Normal, pcB.Normal);
    if numel(normal) ~= numel(points)
        normal = cast([], 'like', points);
    end

    % Only averaging the points in overlapped region
    rangeLimits = overlapRange(pcA, pcB);
    
    if ~isempty(rangeLimits)
        % Apply grid filter to each property
        [points, color, normal] = visionVoxelGridFilter(points, color, normal, gridStep, rangeLimits);
    end
    
    ptCloudOut = pointCloud(points, 'Color', color, 'Normal', normal);
end

end

%==========================================================================
% Compute the bounding box of overlapped region
%==========================================================================
function rangeLimits = overlapRange(pcA, pcB)
xlimA = pcA.XLimits;
ylimA = pcA.YLimits;
zlimA = pcA.ZLimits;

xlimB = pcB.XLimits;
ylimB = pcB.YLimits;
zlimB = pcB.ZLimits;

if (xlimA(1) > xlimB(2) || xlimA(2) < xlimB(1) || ...
    ylimA(1) > ylimB(2) || ylimA(2) < ylimB(1) || ...    
    zlimA(1) > zlimB(2) || zlimA(2) < zlimB(1))
    % No overlap
    rangeLimits = [];
else
    rangeLimits = [ max(xlimA(1),xlimB(1)), min(xlimA(2),xlimB(2)) ...
                    max(ylimA(1),ylimB(1)), min(ylimA(2),ylimB(2)) ...
                    max(zlimA(1),zlimB(1)), min(zlimA(2),zlimB(2))];
end
end