Subversion Repositories seema-scanner

Rev

Rev 241 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 241 Rev 243
1 
function [alignment] = groupwiseOrthogonalProcrustes(P, initialAlign)
 1 
function [alignment] = groupwiseOrthogonalProcrustes(P, initialAlign)
2 
%groupwiseOrthogonalProcrustes Computes rototranslations to bring Np
 2 
%groupwiseOrthogonalProcrustes Computes rototranslations to bring Np
3 
% matched point sets into alignment using non-linear optimization.
 3 
% matched point sets into alignment using non-linear optimization.
4 
%
 4 
%
5 
% Input:
 5 
% Input:
6 
%   P: (Np x Nc) cell array containing point coordinates in matched order.
 6 
%   P: (Np x Nc) cell array containing point coordinates in matched order.
7 
%       empty cells indicate that the corresponding point set does not
 7 
%       empty cells indicate that the corresponding point set does not
8 
%       include a point matched to that cluster.
 8 
%       include a point matched to that cluster.
9 
%   initialAlignment: a Np length struct with fields Rotation (3x3 matrix)
9 
%   initialAlignment: a Np length struct with fields Rotation (3x3 matrix)
10 
%       and Translation (3-vector) for each of the point sets.
 10 
%       and Translation (3-vector) for each of the point sets.
11 
%
 11 
%
12 
% Output:
 12 
% Output:
13 
%   alignment: a struct with fields Rotation (3x3 matrix) and Translation
 13 
%   alignment: a struct with fields Rotation (3x3 matrix) and Translation
14 
%       (3-vector) for each of the point sets.
 14 
%       (3-vector) for each of the point sets.
15 
%
 15 
%
16 
 
 16 
 
17 
Np = size(P, 1);
 17 
Np = size(P, 1);
18 
assert(length(initialAlign) >= Np);
 18 
assert(length(initialAlign) >= Np);
19 
 
 19 
 
20 
% parameter vector contains relative axis angle and translations for all Np
 20 
% parameter vector contains relative axis angle and translations for all Np
21 
% point sets
 21 
% point sets
22 
xInit = zeros(6*Np, 1);
 22 
xInit = zeros(6*Np, 1);
23 
 
 23 
 
24 
% for i=1:Np
 24 
% for i=1:Np
25 
%     [w, theta] = rmat2axis(initialAlign(i).Rotation);
 25 
%     [w, theta] = rmat2axis(initialAlign(i).Rotation);
26 
%     xInit((i-1)*6+1:(i-1)*6+3) = w*theta;
 26 
%     xInit((i-1)*6+1:(i-1)*6+3) = w*theta;
27 
%     xInit((i-1)*6+4:(i-1)*6+6) = initialAlign(i).Translation;
 27 
%     xInit((i-1)*6+4:(i-1)*6+6) = initialAlign(i).Translation;
28 
% end
 28 
% end
29 
 
 29 
 
30 
figure;
 - 
 
31 
%options = optimset('Algorithm', 'levenberg-marquardt', 'Display', 'iter-detailed', 'OutputFcn', @outfun, 'MaxIter', 100, 'TolFun', 0, 'TolX', 0);
 - 
 
32 
options = optimoptions('lsqnonlin', 'Display', 'iter-detailed');
 30 
options = optimoptions('lsqnonlin', 'Display', 'iter-detailed');
33 
[x, ~, ~] = lsqnonlin(@orthProcFun, xInit, [], [], options);
 31 
[x, ~, ~] = lsqnonlin(@orthProcFun, xInit, [], [], options);
34 
 
 32 
 
35 
alignment = struct('Rotation', {}, 'Translation', {});
 33 
alignment = struct('Rotation', {}, 'Translation', {});
36 
for i=1:Np
 34 
for i=1:Np
37 
    wi = x((i-1)*6+1:(i-1)*6+3);
 35 
    wi = x((i-1)*6+1:(i-1)*6+3);
38 
    Ri = initialAlign(i).Rotation * axis2rmat(wi, norm(wi));
 36 
    Ri = initialAlign(i).Rotation * axis2rmat(wi, norm(wi));
39 
    Ti =  initialAlign(i).Translation + x((i-1)*6+4:(i-1)*6+6);
 37 
    Ti =  initialAlign(i).Translation + x((i-1)*6+4:(i-1)*6+6);
40 
    alignment(i).Rotation = Ri;
 38 
    alignment(i).Rotation = Ri;
41 
    alignment(i).Translation =Ti;
 39 
    alignment(i).Translation =Ti;
42 
end
 40 
end
43 
 
 41 
 
44 
 
 42 
 
45 
    % objective function
 43 
    % objective function
46 
    function e = orthProcFun(x)
 44 
    function e = orthProcFun(x)
47 
 
 45 
 
48 
        Nc = size(P, 2);
 46 
        Nc = size(P, 2);
49 
 
 47 
 
50 
        % transform all points according to current x
 48 
        % transform all points according to current x
51 
        Pbar = cell(size(P));
 49 
        Pbar = cell(size(P));
52 
        for j=1:Np
 50 
        for j=1:Np
53 
            wi = x((j-1)*6+1:(j-1)*6+3);
 51 
            wi = x((j-1)*6+1:(j-1)*6+3);
54 
            Ri = initialAlign(j).Rotation * axis2rmat(wi, norm(wi));
 52 
            Ri = initialAlign(j).Rotation * axis2rmat(wi, norm(wi));
55 
            Ti = initialAlign(j).Translation + x((j-1)*6+4:(j-1)*6+6);
 53 
            Ti = initialAlign(j).Translation + x((j-1)*6+4:(j-1)*6+6);
56 
            for k=1:Nc
 54 
            for k=1:Nc
57 
                if(not(isempty(P{j,k})))
 55 
                if(not(isempty(P{j,k})))
58 
                    Pbar{j,k} = P{j,k}*Ri' + Ti';
 56 
                    Pbar{j,k} = P{j,k}*Ri' + Ti';
59 
                end
 57 
                end
60 
            end
 58 
            end
61 
        end
 59 
        end
62 
 
 60 
 
63 
        % include all pairwise distances
 61 
        % include all pairwise distances
64 
        e = [];
 62 
        e = [];
65 
        for j=1:Nc
63 
        for j=1:Nc
66 
            % points in current cluster
 64 
            % points in current cluster
67 
            c = cat(1, Pbar{:,j});
 65 
            c = cat(1, Pbar{:,j});
68 
            Ncj = size(c, 1);
 66 
            Ncj = size(c, 1);
69
67
70 
            if(Ncj < 2)
 68 
            if(Ncj < 2)
71 
                continue;
 69 
                continue;
72 
            end
 70 
            end
73
71
74 
            for k=1:Ncj-1
 72 
            for k=1:Ncj-1
75
73
76 
                for l=k+1:Ncj
 74 
                for l=k+1:Ncj
77
75
78 
                    dVec = c(k,:) - c(l,:);
 76 
                    dVec = c(k,:) - c(l,:);
79
77
80 
                    e(end+1:end+3) = dVec;
 78 
                    e(end+1:end+3) = dVec;
81 
                end
 79 
                end
82 
            end
 80 
            end
83 
 
 81 
 
84 
        end
 82 
        end
85 
 
 83 
 
86 
    end
 84 
    end
87 
 
 85 
 
88 
    % output function called at every iteration
 86 
    % output function called at every iteration
89 
    function stop = outfun(x, optimValues, ~)
 87 
    function stop = outfun(x, optimValues, ~)
90 
        %display(x);
 88 
        %display(x);
91
89
92 
        hold('on');
 90 
        hold('on');
93 
        plot(optimValues.residual);
 91 
        plot(optimValues.residual);
94 
        drawnow;
 92 
        drawnow;
95 
        stop = false;
 93 
        stop = false;
96 
    end
 94 
    end
97 
 
 95 
 
98 
end
 96 
end
99 
 
 97 
 
100 
function [w, theta] = rmat2axis(R)
 98 
function [w, theta] = rmat2axis(R)
101 
 
 99 
 
102 
w = zeros(3, 1);
 100 
w = zeros(3, 1);
103 
%theta = zeros(1, 1);
 101 
%theta = zeros(1, 1);
104 
 
 102 
 
105 
[V,D] = eig(R);
 103 
[V,D] = eig(R);
106 
[~,ix] = min(abs(diag(D)-1));
104 
[~,ix] = min(abs(diag(D)-1));
107 
 
 105 
 
108 
w(:) = V(:,ix);
106 
w(:) = V(:,ix);
109 
t = [R(3,2)-R(2,3),R(1,3)-R(3,1),R(2,1)-R(1,2)];
 107 
t = [R(3,2)-R(2,3),R(1,3)-R(3,1),R(2,1)-R(1,2)];
110 
 
 108 
 
111 
theta = atan2(t*w(:),trace(R(:,:))-1);
 109 
theta = atan2(t*w(:),trace(R(:,:))-1);
112 
 
 110 
 
113 
if theta<0
 111 
if theta<0
114 
    theta = -theta;
112 
    theta = -theta;
115 
    w(:) = -w(:);
113 
    w(:) = -w(:);
116 
end
 114 
end
117 
end
 115 
end
118 
 
 116 
 
119 
function R = axis2rmat(w, theta)
 117 
function R = axis2rmat(w, theta)
120 
 
 118 
 
121 
P = w*transpose(w);
 119 
P = w*transpose(w);
122 
Q = [0 -w(3) w(2);
 120 
Q = [0 -w(3) w(2);
123 
     w(3) 0 -w(1);
 121 
     w(3) 0 -w(1);
124 
     -w(2) w(1) 0];
 122 
     -w(2) w(1) 0];
125 
 
 123 
 
126 
% using Rodigues' rotation formula
 124 
% using Rodigues' rotation formula
127 
R = P + (eye(3) - P)*cos(theta) + Q*sin(theta);
 125 
R = P + (eye(3) - P)*cos(theta) + Q*sin(theta);
128 
 
 126 
 
129 
% ensure orthonormal matrix
 127 
% ensure orthonormal matrix
130 
[U,~,V] = svd(R);
 128 
[U,~,V] = svd(R);
131 
R = U*V';
 129 
R = U*V';
132 
 
 130 
 
133 
end
 131 
end
134 
 
 132 
 
135 
 
 133 
 
136 
 
 134