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', [], '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 k=setxor(1:Np, i)
                        if(not(isempty(Pbar{k,j})))
                            %e = [e; 1/Npi * norm(Pbar{k,j} - Pbar{i,j})];
                            e = [e; sqrt(1/Ncj) * norm(Pbar{k,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