pcregrigidModified.m

function [tform, movingReg, rmse] = pcregrigidModified(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
    % TODO: Update for A
    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: Weird method for thresholding points which have
    % distances too large. 
    keepInlierA = false(ptCloudA.Count, 1); % initialise 0's for # of pairs
    [~, idx] = sort(dists); % make list of pairs by increasing distance
    keepInlierA(idx(1:upperBound)) = true; % set list of pairs under specific dist to 1
    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, :));
                
    elseif strcmpi(metric,'gicp')
            
            [R, T] = minimizeGICP(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