點雲ICP的matlab內建程式碼
阿新 • • 發佈:2020-11-01
ICP的具體原理各大部落格均有介紹,如果不理解可以google一下
直接上matlab程式碼理解
內建函式是pcregrigid
初次編輯,不太會排版,有點亂。。。
function [tform, movingReg, rmse] = pcregrigid(moving, fixed, varargin) %PCREGRIGID Register two point clouds with ICP algorithm. % tform = PCREGRIGID(moving, fixed) returns the rigid transformation that % registers the moving point cloud with the fixed point cloud. moving and % fixed are pointCloud object. tform is an affine3d object that describes % the rigid 3-D transform. The rigid transformation between the moving % and fixed are estimated by Iterative Closest Point (ICP) algorithm. % Consider downsampling point clouds using pcdownsample before using % PCREGRIGID to improve accuracy and efficiency of registration. % % [tform, movingReg] = PCREGRIGID(moving, fixed) additionally % returns the transformed point cloud, movingReg, that is aligned with % the fixed point cloud. % % [..., rmse] = PCREGRIGID(moving, fixed) additionally returns the % root mean squared error of the Euclidean distance between the aligned % point clouds. % % [...] = PCREGRIGID(...,Name, Value) specifies additional % name-value pairs described below: % % 'Metric' A string used to specify the metric of the % minimization function. The ICP algorithm minimized % the distance between the two point clouds according % to the given metric. Valid strings are % 'pointToPoint' and 'pointToPlane'. Setting Metric % to 'pointToPlane' can reduce the number of % iterations to process. However, this metric % requires extra algorithmic steps within each % iteration. The 'pointToPlane' metric helps % registration of planar surfaces. % % Default: 'pointToPoint' % % 'Extrapolate' A boolean to turn on/off the extrapolation step % that traces out a path in the registration state % space, described in the original paper of ICP by % Besl and McKay (1992). This may reduce the number % of iterations to converge. % % Default: false % % 'InlierRatio' A scalar to specify the percentage of inliers. % During an ICP iteration, every point in the moving % point cloud is matched to its nearest neighbor in % the fixed point cloud. The pair of matched points % is considered as an inlier if its Euclidean % distance falls into the given percentage of the % distribution of matching distance. By default, all % matching pairs are used. % % Default: 1 % % 'MaxIterations' A positive integer to specify the maximum number % of iterations before ICP stops. % % Default: 20 % % 'Tolerance' A 2-element vector, [Tdiff, Rdiff], to specify % the tolerance of absolute difference in translation % and rotation estimated in consecutive ICP % iterations. Tdiff measures the Euclidean distance % between two translation vectors, while Rdiff % measures the angular difference in radians. The % algorithm stops when the average difference between % estimated rigid transformations in the three most % recent consecutive iterations falls below the % specified tolerance value. % % Default: [0.01, 0.009] % % 'InitialTransform' An affine3d object to specify the initial rigid % transformation. This is useful when a coarse % estimation can be provided externally. % % Default: affine3d() % % 'Verbose' Set true to display progress information. % % Default: false % % Notes % ----- % - The registration algorithm is based on Iterative Closest Point (ICP) % algorithm, which is an iterative process. Best performance might % require adjusting the options for different data. % % - Point cloud normals are required by the registration algorithm when % 'pointToPlane' metric is chosen. If the Normal property of the second % input 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 registration with 'pointToPlane' % metric fails, consider calling pcnormals function with a custom value % of K. % % Class Support % ------------- % moving and fixed must be pointCloud object. % % Example: Align two point clouds % -------------------------------- % ptCloud = pcread('teapot.ply'); % figure % pcshow(ptCloud) % title('Teapot') % % % Create a transform object with 30 degree rotation along z-axis and % % translation [5, 5, 10] % A = [cos(pi/6) sin(pi/6) 0 0; ... % -sin(pi/6) cos(pi/6) 0 0; ... % 0 0 1 0; ... % 5 5 10 1]; % tform1 = affine3d(A); % % % Transform the point cloud % ptCloudTformed = pctransform(ptCloud, tform1); % % figure % pcshow(ptCloudTformed) % title('Transformed Teapot') % % % Apply the rigid registration % [tform, ptCloudReg] = pcregrigid(ptCloudTformed, ptCloud, 'Extrapolate', true); % % % Visualize the alignment % pcshowpair(ptCloud, ptCloudReg) % % % Compare the result with the true transformation % disp(tform1.T); % tform2 = invert(tform); % disp(tform2.T); % % See also pointCloud, pctransform, affine3d, pcshow, pcdownsample, % pcshowpair, pcdenoise, pcmerge % Copyright 2014 The MathWorks, Inc. % % References % ---------- % Besl, Paul J.; N.D. McKay (1992). "A Method for Registration of 3-D % Shapes". IEEE Trans. on Pattern Analysis and Machine Intelligence (Los % Alamitos, CA, USA: IEEE Computer Society) 14 (2): 239�256. % % Chen, Yang; Gerard Medioni (1991). "Object modelling by registration of % multiple range images". Image Vision Comput. (Newton, MA, USA: % Butterworth-Heinemann): 145�155 % Validate inputs [metric, doExtrapolate, inlierRatio, maxIterations, tolerance, ... initialTransform, verbose] = validateAndParseOptInputs(moving, fixed, varargin{:}); printer = vision.internal.MessagePrinter.configure(verbose); % A copy of the input with unorganized M-by-3 data ptCloudA = removeInvalidPoints(moving); [ptCloudB, validPtCloudIndices] = removeInvalidPoints(fixed); % At least three points are needed to determine a 3-D transformation if ptCloudA.Count < 3 || ptCloudB.Count < 3 error(message('vision:pointcloud:notEnoughPoints')); end % Normal vector is needed for PointToPlane metric if strcmpi(metric, 'PointToPlane') % Compute the unit normal vector if it is not provided. if isempty(fixed.Normal) fixedCount = fixed.Count; % Use 6 neighboring points to estimate a normal vector. You may use % pcnormals with customized parameter to compute normals upfront. fixed.Normal = surfaceNormalImpl(fixed, 6); ptCloudB.Normal = [fixed.Normal(validPtCloudIndices), ... fixed.Normal(validPtCloudIndices + fixedCount), ... fixed.Normal(validPtCloudIndices + fixedCount * 2)]; end % Remove points if their normals are invalid tf = isfinite(ptCloudB.Normal); validIndices = find(sum(tf, 2) == 3); if numel(validIndices) < ptCloudB.Count [loc, ~, nv] = subsetImpl(ptCloudB, validIndices); ptCloudB = pointCloud(loc, 'Normal', nv); if ptCloudB.Count < 3 error(message('vision:pointcloud:notEnoughPoints')); end end end Rs = zeros(3, 3, maxIterations+1); Ts = zeros(3, maxIterations+1); % Quaternion and translation vector qs = [ones(1, maxIterations+1); zeros(6, maxIterations+1)]; % The difference of quaternion and translation vector in consecutive % iterations dq = zeros(7, maxIterations+1); % The angle between quaternion and translation vectors in consecutive % iterations dTheta = zeros(maxIterations+1, 1); % RMSE Err = zeros(maxIterations+1, 1); % Apply the initial condition. % We use pre-multiplication format in this algorithm. Rs(:,:,1) = initialTransform.T(1:3, 1:3)'; Ts(:,1) = initialTransform.T(4, 1:3)'; qs(:,1) = [vision.internal.quaternion.rotationToQuaternion(Rs(:,:,1)); Ts(:,1)]; locA = ptCloudA.Location; if qs(1) ~= 0 || any(qs(2:end,1)) locA = rigidTransform(ptCloudA.Location, Rs(:,:,1), Ts(:,1)); end stopIteration = maxIterations; upperBound = max(1, round(inlierRatio(1)*ptCloudA.Count)); % Start ICP iterations for i = 1 : maxIterations printer.linebreak; printer.print('--------------------------------------------\n'); printer.printMessage('vision:pointcloud:icpIteration',i); printer.printMessageNoReturn('vision:pointcloud:findCorrespondenceStart'); % Find the correspondence [indices, dists] = multiQueryKNNSearchImpl(ptCloudB, locA, 1); % Remove outliers keepInlierA = false(ptCloudA.Count, 1); [~, idx] = sort(dists); keepInlierA(idx(1:upperBound)) = true; inlierIndicesA = find(keepInlierA); inlierIndicesB = indices(keepInlierA); inlierDist = dists(keepInlierA); if numel(inlierIndicesA) < 3 error(message('vision:pointcloud:notEnoughPoints')); end printer.printMessage('vision:pointcloud:stepCompleted'); if i == 1 Err(i) = sqrt(sum(inlierDist)/length(inlierDist)); end printer.printMessageNoReturn('vision:pointcloud:estimateTransformStart'); % Estimate transformation given correspondences if strcmpi(metric, 'PointToPoint') [R, T] = minimizePointToPointMetric(locA(inlierIndicesA, :), ... ptCloudB.Location(inlierIndicesB, :)); else % PointToPlane [R, T] = minimizePointToPlaneMetric(locA(inlierIndicesA, :), ... ptCloudB.Location(inlierIndicesB, :), ptCloudB.Normal(inlierIndicesB, :)); end % Bad correspondence may lead to singular matrix if any(isnan(T))||any(isnan(R(:))) error(message('vision:pointcloud:singularMatrix')); end % Update the total transformation Rs(:,:,i+1) = R * Rs(:,:,i); Ts(:,i+1) = R * Ts(:,i) + T; printer.printMessage('vision:pointcloud:stepCompleted'); % RMSE locA = rigidTransform(ptCloudA.Location, Rs(:,:,i+1), Ts(:,i+1)); squaredError = sum((locA(inlierIndicesA, :) - ptCloudB.Location(inlierIndicesB, :)).^2, 2); Err(i+1) = sqrt(sum(squaredError)/length(squaredError)); % Convert to vector representation qs(:,i+1) = [vision.internal.quaternion.rotationToQuaternion(Rs(:,:,i+1)); Ts(:,i+1)]; % With extrapolation, we might be able to converge faster if doExtrapolate printer.printMessageNoReturn('vision:pointcloud:updateTransformStart'); extrapolateInTransformSpace; printer.printMessage('vision:pointcloud:stepCompleted'); end % Check convergence % Compute the mean difference in R/T from the recent three iterations. [dR, dT] = getChangesInTransformation; printer.printMessage('vision:pointcloud:checkConverge',num2str(tdiff), num2str(rdiff), num2str(Err(i+1))); % Stop ICP if it already converges if dT <= tolerance(1) && dR <= tolerance(2) stopIteration = i; break; end end % Make the R to be orthogonal as much as possible R = Rs(:,:,stopIteration+1)'; [U, ~, V] = svd(R); R = U * V'; tformMatrix = [R, zeros(3,1);... Ts(:, stopIteration+1)', 1]; tform = affine3d(tformMatrix); rmse = Err(stopIteration+1); printer.linebreak; printer.print('--------------------------------------------\n'); printer.printMessage('vision:pointcloud:icpSummary',stopIteration, num2str(rmse)); if nargout >= 2 movingReg = pctransform(moving, tform); end %====================================================================== % Nested function to perform extrapolation % Besl, P., & McKay, N. (1992). A method for registration of 3-D shapes. % IEEE Transactions on pattern analysis and machine intelligence, p245. %====================================================================== function extrapolateInTransformSpace dq(:,i+1) = qs(:,i+1) - qs(:,i); n1 = norm(dq(:,i)); n2 = norm(dq(:,i+1)); dTheta(i+1) = (180/pi)*acos(dot(dq(:,i),dq(:,i+1))/(n1*n2)); angleThreshold = 10; scaleFactor = 25; if i > 2 && dTheta(i+1) < angleThreshold && dTheta(i) < angleThreshold d = [Err(i+1), Err(i), Err(i-1)]; v = [0, -n2, -n1-n2]; vmax = scaleFactor * n2; dv = extrapolate(v,d,vmax); if dv ~= 0 q = qs(:,i+1) + dv * dq(:,i+1)/n2; q(1:4) = q(1:4)/norm(q(1:4)); % Update transformation and data qs(:,i+1) = q; Rs(:,:,i+1) = vision.internal.quaternion.quaternionToRotation(q(1:4)); Ts(:,i+1) = q(5:7); locA = rigidTransform(ptCloudA.Location, Rs(:,:,i+1), Ts(:,i+1)); end end end %====================================================================== % Nested function to compute the changes in rotation and translation %====================================================================== function [dR, dT] = getChangesInTransformation dR = 0; dT = 0; count = 0; for k = max(i-2,1):i % Rotation difference in radians rdiff = acos(dot(qs(1:4,k),qs(1:4,k+1))/(norm(qs(1:4,k))*norm(qs(1:4,k+1)))); % Euclidean difference tdiff = sqrt(sum((Ts(:,k)-Ts(:,k+1)).^2)); dR = dR + rdiff; dT = dT + tdiff; count = count + 1; end dT = dT/count; dR = dR/count; end end %========================================================================== % Parameter validation %========================================================================== function [metric, doExtrapolate, inlierRatio, maxIterations, tolerance, ... initialTransform, verbose] = validateAndParseOptInputs(moving, fixed, varargin) if ~isa(moving, 'pointCloud') error(message('vision:pointcloud:notPointCloudObject','moving')); end if ~isa(fixed, 'pointCloud') error(message('vision:pointcloud:notPointCloudObject','fixed')); end persistent p; if isempty(p) % Set input parser defaults = struct(... 'Metric', 'PointToPoint', ... 'Extrapolate', false, ... 'InlierRatio', 1.0,... 'MaxIterations', 20,... 'Tolerance', [0.01, 0.009],... 'InitialTransform', affine3d(),... 'Verbose', false); p = inputParser; p.CaseSensitive = false; p.addParameter('Metric', defaults.Metric); p.addParameter('Extrapolate', defaults.Extrapolate, ... @(x)validateattributes(x,{'logical'}, {'scalar','nonempty'})); p.addParameter('InlierRatio', defaults.InlierRatio, ... @(x)validateattributes(x,{'single', 'double'}, {'real','nonempty','scalar','>',0,'<=',1})); p.addParameter('MaxIterations', defaults.MaxIterations, ... @(x)validateattributes(x,{'single', 'double'}, {'scalar','integer'})); p.addParameter('Tolerance', defaults.Tolerance, ... @(x)validateattributes(x,{'single', 'double'}, {'real','nonnegative','numel', 2})); p.addParameter('InitialTransform', defaults.InitialTransform, ... @(x)validateattributes(x,{'affine3d'}, {'scalar'})); p.addParameter('Verbose', defaults.Verbose, ... @(x)validateattributes(x,{'logical'}, {'scalar','nonempty'})); parser = p; else parser = p; end parser.parse(varargin{:}); metric = validatestring(parser.Results.Metric, {'PointToPoint', 'PointToPlane'}, mfilename, 'Metric'); doExtrapolate = parser.Results.Extrapolate; inlierRatio = parser.Results.InlierRatio; maxIterations = parser.Results.MaxIterations; tolerance = parser.Results.Tolerance; initialTransform = parser.Results.InitialTransform; if ~(isRigidTransform(initialTransform)) error(message('vision:pointcloud:rigidTransformOnly')); end verbose = parser.Results.Verbose; end %========================================================================== % Determine if transformation is rigid transformation %========================================================================== function tf = isRigidTransform(tform) singularValues = svd(tform.T(1:tform.Dimensionality,1:tform.Dimensionality)); tf = max(singularValues)-min(singularValues) < 100*eps(max(singularValues(:))); tf = tf && abs(det(tform.T)-1) < 100*eps(class(tform.T)); end %========================================================================== function B = rigidTransform(A, R, T) B = A * R'; B(:,1) = B(:,1) + T(1); B(:,2) = B(:,2) + T(2); B(:,3) = B(:,3) + T(3); end %========================================================================== % Solve the following minimization problem: % min_{R, T} sum(|R*p+T-q|^2) % % p, q are all N-by-3 matrix % % The problem is solved by SVD %========================================================================== function [R, T] = minimizePointToPointMetric(p, q) n = size(p, 1); m = size(q, 1); % Find data centroid and deviations from centroid pmean = sum(p,1)/n; p2 = p - repmat(pmean, n, 1); qmean = sum(q,1)/m; q2 = q - repmat(qmean, m, 1); % Covariance matrix C = p2'*q2; [U,~,V] = svd(C); % Handle the reflection case R = V*diag([1 1 sign(det(U*V'))])*U'; % Compute the translation T = qmean' - R*pmean'; end %========================================================================== % Solve the following minimization problem: % min_{R, T} sum(|dot(R*p+T-q,nv)|^2) % % p, q, nv are all N-by-3 matrix, and nv is the unit normal at q % % Here the problem is solved by linear approximation to the rotation matrix % when the angle is small. %========================================================================== function [R, T] = minimizePointToPlaneMetric(p, q, nv) % Set up the linear system cn = [cross(p,nv,2),nv]; C = cn'*cn; qp = q-p; b = [sum(sum(qp.*repmat(cn(:,1),1,3).*nv, 2)); sum(sum(qp.*repmat(cn(:,2),1,3).*nv, 2)); sum(sum(qp.*repmat(cn(:,3),1,3).*nv, 2)); sum(sum(qp.*repmat(cn(:,4),1,3).*nv, 2)); sum(sum(qp.*repmat(cn(:,5),1,3).*nv, 2)); sum(sum(qp.*repmat(cn(:,6),1,3).*nv, 2))]; % X is [alpha, beta, gamma, Tx, Ty, Tz] X = C\b; cx = cos(X(1)); cy = cos(X(2)); cz = cos(X(3)); sx = sin(X(1)); sy = sin(X(2)); sz = sin(X(3)); R = [cy*cz, sx*sy*cz-cx*sz, cx*sy*cz+sx*sz; cy*sz, cx*cz+sx*sy*sz, cx*sy*sz-sx*cz; -sy, sx*cy, cx*cy]; T = X(4:6); end %========================================================================== % Extrapolation in quaternion space. Details are found in: % Besl, P., & McKay, N. (1992). A method for registration of 3-D shapes. % IEEE Transactions on pattern analysis and machine intelligence, 239-256. %========================================================================== function dv = extrapolate(v,d,vmax) p1 = polyfit(v,d,1); % linear fit p2 = polyfit(v,d,2); % parabolic fit v1 = -p1(2)/p1(1); % linear zero crossing point v2 = -p2(2)/(2*p2(1)); % polynomial top point if (issorted([0 v2 v1 vmax]) || issorted([0 v2 vmax v1])) % Parabolic update dv = v2; elseif (issorted([0 v1 v2 vmax]) || issorted([0 v1 vmax v2])... || (v2 < 0 && issorted([0 v1 vmax]))) % Line update dv = v1; elseif (v1 > vmax && v2 > vmax) % Maximum update dv = vmax; else % No extrapolation dv = 0; end end