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