gusucode.com > vision工具箱matlab源码程序 > vision/pcfitcylinder.m
function [model, inlierIndices, outlierIndices, rmse] = pcfitcylinder(varargin)
%PCFITCYLINDER Fit cylinder to a 3-D point cloud.
% model = PCFITCYLINDER(ptCloud, maxDistance) fits a cylinder to the
% point cloud, ptCloud. The cylinder is described by a cylinderModel
% object. maxDistance is a maximum allowed distance from an inlier
% point to the cylinder. This function uses the M-estimator SAmple
% Consensus (MSAC) algorithm to find the cylinder.
%
% model = PCFITCYLINDER(..., referenceVector) fits a cylinder to the
% point cloud with additional orientation constraints. referenceVector is
% a 1-by-3 vector used as a reference orientation.
%
% model = PCFITCYLINDER(..., referenceVector, maxAngularDistance) fits a
% cylinder to the point cloud with additional orientation constraints.
% maxAngularDistance specifies the maximum allowed absolute angular
% distance in degrees between the direction of fitted cylinder and the
% reference orientation. If it is not specified, the default is 5
% degrees.
%
% [..., inlierIndices, outlierIndices] = PCFITCYLINDER(...) additionally
% returns linear indices to the inlier and outlier points in ptCloud.
%
% [..., rmse] = PCFITCYLINDER(...) additionally returns root mean square
% error of the distance of inlier points to the model.
%
% [...] = PCFITCYLINDER(..., 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
%
% Notes:
% ------
% - Point cloud normals are required by the fitting algorithm. If the
% Normal property of the input ptCloud is empty, the function fills it.
%
% - When the Normal property is filled automatically, the number of
% points, K, to fit local plane is set to 6. This default may not work
% under all circumstances. If the fitting fails, consider calling
% pcnormals function with a custom value of K.
%
% Class Support
% -------------
% ptCloud must be pointCloud object.
%
% Example: Detect a cylinder 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-cylinder distance (5mm) for cylinder fitting
% maxDistance = 0.005;
%
% % Set the roi to constrain the search
% roi = [0.4, 0.6, -inf, 0.2, 0.1, inf];
% sampleIndices = findPointsInROI(ptCloud, roi);
%
% % Set the orientation constraint
% referenceVector = [0, 0, 1];
%
% % Detect the cylinder and extract it from the point cloud
% [model, inlierIndices] = pcfitcylinder(ptCloud, maxDistance,...
% referenceVector, 'SampleIndices', sampleIndices);
% pc = select(ptCloud, inlierIndices);
%
% % Plot the cylinder
% figure
% pcshow(pc)
% title('Cylinder Point Cloud')
%
% See also pointCloud, pointCloud>findPointsInROI, cylinderModel, pcshow,
% pcfitplane, pcfitsphere
% 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 two points with normal to fit a cylinder
sampleSize = 2;
ransacParams.sampleSize = sampleSize;
% Compute normal property if it is not available
if isempty(ptCloud.Normal)
% Use 6 neighboring points to estimate a normal vector
ptCloud.Normal = surfaceNormalImpl(ptCloud, 6);
end
% 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 = @fitCylinder;
ransacFuncs.evalFunc = @evalCylinder;
% The initial model parameters are expressed as [x,y,z, dx,dy,dz, r],
% where (x,y,z) is a point on the axis, (dx,dy,dz) specifies the
% direction of the axis, and r is the radius.
if isempty(referenceVector)
ransacFuncs.checkFunc = @checkCylinder;
[isFound, modelParams] = vision.internal.ransac.msac([pc.Location,...
pc.Normal], ransacParams, ransacFuncs);
else
% Fit the cylinder with orientation constraint
ransacFuncs.checkFunc = @checkOrientedCylinder;
denorm = sqrt(dot(referenceVector,referenceVector));
normAxis = referenceVector ./ denorm;
[isFound, modelParams] = vision.internal.ransac.msac(...
[pc.Location, pc.Normal],ransacParams, ransacFuncs,...
normAxis, maxAngularDistance);
if isFound
% Adjust the cylinder axis 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(4:6))));
angle = abs(acos(a));
if angle > pi/2
modelParams(4:6) = -modelParams(4:6);
end
end
end
if ~isFound
status = statusCode.NotEnoughInliers;
else
% Re-evaluate the best model
if ~isempty(sampleIndices)
[pc, validPtCloudIndices] = removeInvalidPoints(ptCloud);
end
distances = evalCylinder(modelParams, pc.Location);
inlierIndices = validPtCloudIndices(distances < ransacParams.maxDistance);
% Convert to end-points expression
modelParams = convertToFiniteCylinderModel(modelParams, ptCloud, inlierIndices);
end
end
if status == statusCode.NoError
% Construct the cylinder object
model = cylinderModel(modelParams);
else
model = cylinderModel.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
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
%==========================================================================
% Cylinder equations:
% axis line: v(p) = p0 + (p - p0)*k
% point-to-axis distance: r^2 = ||(p2-p1)x(p1-p0)|| / ||p2-p1||
% The cross product of two normals determines the direction of the axis.
%==========================================================================
function model = fitCylinder(points, varargin)
p1 = points(1,1:3);
n1 = points(1,4:6);
p2 = points(2,1:3);
n2 = points(2,4:6);
w = p1 + n1 - p2;
a = n1 * n1';
b = n1 * n2';
c = n2 * n2';
d = n1 * w';
e = n2 * w';
D = a * c - b * b;
if abs(D) < 1e-5
s = 0;
if b > c
t = d / b;
else
t = e / c;
end
else
s = (b * e - c * d) / D;
t = (a * e - b * d) / D;
end
% P0 is a point on the axis
p0 = p1 + n1 + s * n1;
% dp is a normalized vector for the direction of the axis
dp = p2 + t * n2 - p0;
dp = dp / norm(dp);
p1p0 = p1 - p0;
p2p1 = dp;
c = [p1p0(2)*p2p1(3) - p1p0(3)*p2p1(2), ...
p1p0(3)*p2p1(1) - p1p0(1)*p2p1(3), ...
p1p0(1)*p2p1(2) - p1p0(2)*p2p1(1)];
% p2p1 is a unit vector, so the denominator is not needed
r = sqrt(sum(c.*c, 2));
model = [p0, dp, r];
%==========================================================================
% Calculate the distance from the point P0 to the axis P2-P1
% D = ||(P2-P1) x (P1-P0)|| / ||P2-P1||
% The distance from P0 to the cylinder is ||D - r||
%==========================================================================
function dis = evalCylinder(model, points, varargin)
p1p0 = [points(:,1)-model(1), points(:,2)-model(2), points(:,3)-model(3)];
p2p1 = model(4:6);
c = [p1p0(:,2)*p2p1(3) - p1p0(:,3)*p2p1(2), ...
p1p0(:,3)*p2p1(1) - p1p0(:,1)*p2p1(3), ...
p1p0(:,1)*p2p1(2) - p1p0(:,2)*p2p1(1)];
% p2p1 is a unit vector, so the denominator is not needed
D = sum(c.*c, 2);
dis = abs(sqrt(D) - model(7));
%==========================================================================
% Validate the cylinder coefficients
%==========================================================================
function isValid = checkCylinder(model)
isValid = (numel(model) == 7 & all(isfinite(model)));
%==========================================================================
% Validate the cylinder coefficients with orientation constraints
%==========================================================================
function isValid = checkOrientedCylinder(model, normAxis, threshold)
isValid = checkCylinder(model);
if isValid
a = min(1, max(-1, normAxis*model(4:6)'));
angle = abs(acos(a));
angle = min(angle, pi-angle);
isValid = (angle < threshold);
end
%==========================================================================
% Convert the point-axis format to end-points format
%
% The MSAC algorithm estimates a point on the axis and the direction of
% axis. This function projects all inliers to the axis, computes two end
% points, and use these two end-points along with the radius to describe a
% finite cylinder.
%==========================================================================
function modelParams = convertToFiniteCylinderModel(modelParams, ptCloud, inlierIndices)
% Get the direction of cylinder axis
dp = modelParams(4:6);
% Get the inlier points
points = subsetImpl(ptCloud, inlierIndices);
% Get a point on the axis
p0 = modelParams(1:3);
% Describe the axis as a line equation: p0 + k * dp, and find the
% projections of inlier points on this line
k = sum(points.*repmat(dp, numel(inlierIndices), 1),2) - p0 * dp';
% Find the two extreme points
pa = p0 + min(k) * dp;
pb = p0 + max(k) * dp;
% Set to the parameters required by cylinderModel object
modelParams(1:6) = [pa, pb];