Subversion Repositories seema-scanner

function [alignment] = groupwiseOrthogonalProcrustes(P, initialAlign)

%groupwiseOrthogonalProcrustes Computes rototranslations to bring Np

% matched point sets into alignment using non-linear optimization.

%

% Input:

%   P: (Np x Nc) cell array containing point coordinates in matched order.

%       empty cells indicate that the corresponding point set does not

%       include a point matched to that cluster.

%   initialAlignment: a Np length struct with fields Rotation (3x3 matrix)  

%       and Translation (3-vector) for each of the point sets.

%

% Output:

%   alignment: a struct with fields Rotation (3x3 matrix) and Translation

%       (3-vector) for each of the point sets.

%

Np = size(P, 1);

assert(length(initialAlign) >= Np);

% parameter vector contains relative axis angle and translations for all Np

% point sets

xInit = zeros(6*Np, 1);

% for i=1:Np

%     [w, theta] = rmat2axis(initialAlign(i).Rotation);

%     xInit((i-1)*6+1:(i-1)*6+3) = w*theta;

%     xInit((i-1)*6+4:(i-1)*6+6) = initialAlign(i).Translation;

% end

options = optimoptions('lsqnonlin', 'Display', 'iter-detailed');

[x, ~, ~] = lsqnonlin(@orthProcFun, xInit, [], [], options);

alignment = struct('Rotation', {}, 'Translation', {});

for i=1:Np

    wi = x((i-1)*6+1:(i-1)*6+3);

    Ri = initialAlign(i).Rotation * axis2rmat(wi, norm(wi));

    Ti =  initialAlign(i).Translation + x((i-1)*6+4:(i-1)*6+6);

    alignment(i).Rotation = Ri;

    alignment(i).Translation =Ti;

end

    % objective function

    function e = orthProcFun(x)

        Nc = size(P, 2);

        % transform all points according to current x

        Pbar = cell(size(P));

        for j=1:Np

            wi = x((j-1)*6+1:(j-1)*6+3);

            Ri = initialAlign(j).Rotation * axis2rmat(wi, norm(wi));

            Ti = initialAlign(j).Translation + x((j-1)*6+4:(j-1)*6+6);

            for k=1:Nc

                if(not(isempty(P{j,k})))

                    Pbar{j,k} = P{j,k}*Ri' + Ti';

                end

            end

        end

        % include all pairwise distances

        e = [];

        for j=1:Nc   

            % points in current cluster

            c = cat(1, Pbar{:,j});

            Ncj = size(c, 1);

            if(Ncj < 2)

                continue;

            end

            for k=1:Ncj-1

                for l=k+1:Ncj

                    dVec = c(k,:) - c(l,:);

                    e(end+1:end+3) = dVec;

                end

            end

        end

    end

    % output function called at every iteration

    function stop = outfun(x, optimValues, ~)

        %display(x);

        hold('on');

        plot(optimValues.residual);

        drawnow;

        stop = false;

    end

end

function [w, theta] = rmat2axis(R)

w = zeros(3, 1);

%theta = zeros(1, 1);

[V,D] = eig(R);

[~,ix] = min(abs(diag(D)-1)); 

w(:) = V(:,ix); 

t = [R(3,2)-R(2,3),R(1,3)-R(3,1),R(2,1)-R(1,2)];

theta = atan2(t*w(:),trace(R(:,:))-1);

if theta<0

    theta = -theta; 

    w(:) = -w(:); 

end

end

function R = axis2rmat(w, theta)

P = w*transpose(w);

Q = [0 -w(3) w(2);

     w(3) 0 -w(1);

     -w(2) w(1) 0];

% using Rodigues' rotation formula

R = P + (eye(3) - P)*cos(theta) + Q*sin(theta);

% ensure orthonormal matrix

[U,~,V] = svd(R);

R = U*V';

end