regpt2surf.m

function [A, b, newpos] = regpt2surf(node, elem, p, pmask, A0, b0, cmask, maxiter)
%  [A,b,newpos]=regpt2surf(node,elem,p,pmask,A0,b0,cmask,maxiter)
%  Perform point cloud registration to a triangular surface
%  (surface can be either triangular or cubic), Gauss-Newton method
%  is used for the calculation
%
%  author: Qianqian Fang <q.fang at neu.edu>
%  date: 12/12/2008
%
% parameters:
%      node: node coordinate of the surface mesh (nn x 3)
%      elem: element list of the surface mesh (3 columns for
%            triangular mesh, 4 columns for cubic surface mesh)
%      p: points to be registered, 3 columns for x,y and z respectively
%      pmask: a mask vector with the same length as p, determines the
%         method to handle the point, if pmask(i)=-1, the point is a free
%         node and can be move by the optimization, if pmask(i)=0, the
%         point is fixed; if pmask(i)=n>0, the distance between p(i,:)
%         and node(n,:) will be part of the object function and be optimized
%      A0: a 3x3 matrix, as the initial guess for the affine A matrix (rotation&scaling)
%      b0: a 3x1 vector, as the initial guess for the affine b vector (translation)
%      cmask: a binary 12x1 vector, determines which element of [A(:);b] will be optimized
%          if cmask(i)=0, the corresponding coefficient will not be updated
%      maxiter: a integer, specifying the optimization iterations
%
% outputs:
%      A: 3x3 matrix, the updated affine A matrix
%      b: 3x1 vector, the updated affine b vector
%      newpos: the registered positions for p, newpos=A*p'+b
%
% Please find more information at http://iso2mesh.sf.net/cgi-bin/index.cgi?metch
%
% this function is part of "metch" toobox, see COPYING for license

A = A0;
b = b0(:);

% for simplicity, I wrap A and b into one single vector C
C = [A(:); b];

delta = 1e-4;
newpos = (reshape(C(1:9), 3, 3) * p' + repmat(C(end - 2:end), 1, size(p, 1)))';
nv = nodesurfnorm(node, elem);
clen = length(C);
cuplist = find(cmask == 1);
pfree = find(pmask < 0);
pfix = find(pmask > 0);

% start Gauss-Newton iterations
for iter = 1:maxiter

    % calculate the current residual: the sum of distances to the surface
    dist0 = zeros(length(pfree) + length(pfix), 1);
    if (~isempty(pfree))
        [dist0(pfree), cn0] = dist2surf(node, nv, newpos(pfree, :));
    end
    if (~isempty(pfix))
        fixdist = node(pmask(pfix), :) - newpos(pfix, :);
        dist0(pfix) = sqrt(sum(fixdist .* fixdist, 2));
    end
    fprintf('iter=%d error=%f\n', iter, sum(abs(dist0)));

    % build the Jacobian (sensitivity) matrix
    J = zeros(length(dist0), length(C));
    for i = 1:clen
        if (cmask(i) == 0)
            continue
        end
        dC = C;
        if (C(i))
            dC(i) = C(i) * (1 + delta);
        else
            dC(i) = C(i) + delta;
        end
        newp = (reshape(dC(1:9), 3, 3) * p' + repmat(dC(end - 2:end), 1, size(p, 1)))';

        dist = zeros(length(pfree) + length(pfix), 1);
        if (~isempty(pfree))
            if (length(cn0) == length(pfree))
                dist(pfree) = dist2surf(node, nv, newp(pfree, :), cn0);
            else
                dist(pfree) = dist2surf(node, nv, newp(pfree, :));
            end
        end
        if (~isempty(pfix))
            fixdist = node(pmask(pfix), :) - newp(pfix, :);
            dist(pfix) = sqrt(sum(fixdist .* fixdist, 2));
        end
        % J=dL/dC
        J(:, i) = (dist - dist0) / (dC(i) - C(i));
    end

    % weight the matrix (normalization)
    wj = sqrt(sum(J .* J));
    J(:, cuplist) = J(:, cuplist) ./ repmat(wj(cuplist), length(dist0), 1);

    % calculate the update: J*dC=dL
    dC = (J(:, cuplist) \ dist0) ./ wj(cuplist)';
    C(cuplist) = C(cuplist) - 0.5 * dC;

    % get the updated positions with the calculated A and b
    newpos = (reshape(C(1:9), 3, 3) * p' + repmat(C(end - 2:end), 1, size(p, 1)))';
end

A = reshape(C(1:9), 3, 3);
b = C(end - 2:end);