Subversion Repositories seema-scanner

function [alignment] = groupwiseOrthogonalProcrustes(P, initialAlign)

%groupwiseOrthogonalProcrustes Computes rototranslations to bring N

% 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 N 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);

xInit = zeros(6*Np, 1);

for k=1:Np

    [wi, thetai] = rmat2axis(initialAlign(k).Rotation);

    xInit((k-1)*6+1:(k-1)*6+3) = wi*thetai;

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

end

options = optimset('Algorithm', 'levenberg-marquardt', 'Display', 'iter-detailed', 'OutputFcn', @outfun, 'MaxIter', 100, 'TolFun', 0, 'TolX', 0);

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

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

for k=1:Np

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

    Ri = axis2rmat(wi, norm(wi));

    Ti = x((k-1)*6+4:(k-1)*6+6);

    alignment(k).Rotation = Ri;

    alignment(k).Translation = Ti;

end

    % objective function

    function e = orthProcFun(x)

        Np = size(P, 1);

        Nc = size(P, 2);

        % transform all points according to current x

        Pbar = cell(size(P));

        for i=1:Np

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

            Ri = axis2rmat(wi, norm(wi));

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

            for j=1:Nc

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

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

                end

            end

        end

        % include all pairwise distances, normalizing for each point sets

        e = [];

        for i=1:Np   

            % number of points in current point set

            %Npi = size(cat(1, Pbar{i,:}), 1);

            for j=1:Nc

                % number of points in current cluster

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

                if(not(isempty(Pbar{i,j})))

                    for l=setxor(1:Np, i)

                        if(not(isempty(Pbar{l,j})))

                            %e = [e; 1/Npi * norm(Pbar{k,j} - Pbar{i,j})];

                            e = [e; sqrt(1/Ncj) * norm(Pbar{l,j} - Pbar{i,j})];

                            %e = [e; norm(Pbar{k,j} - Pbar{i,j})];

                        end      

                    end   

                end

            end

        end

    end

    % output function called at every iteration

    function stop = outfun(x, optimValues, ~)

        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(1); 

    w(:) = -w(:); 

end

end

function R = axis2rmat(w, theta)

if(theta > 0.0001)

    w = w./norm(w);

end

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