Subversion Repositories seema-scanner

Rev

Rev 238 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 238 Rev 241
Line 1... Line 1...
1 
function [alignment] = groupwiseOrthogonalProcrustes(P, initialAlign)
 1 
function [alignment] = groupwiseOrthogonalProcrustes(P, initialAlign)
2 
%groupwiseOrthogonalProcrustes Computes rototranslations to bring N
 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 N 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
- 
 
 21 
% point sets
20 
xInit = zeros(6*Np, 1);
 22 
xInit = zeros(6*Np, 1);
21 
 
 23 
 
22 
for k=1:Np
 24 
% for i=1:Np
23 
    [wi, thetai] = rmat2axis(initialAlign(k).Rotation);
 25 
%     [w, theta] = rmat2axis(initialAlign(i).Rotation);
24 
    xInit((k-1)*6+1:(k-1)*6+3) = wi*thetai;
 26 
%     xInit((i-1)*6+1:(i-1)*6+3) = w*theta;
25
- 
 
26 
    xInit((k-1)*6+4:(k-1)*6+6) = initialAlign(k).Translation;
 27 
%     xInit((i-1)*6+4:(i-1)*6+6) = initialAlign(i).Translation;
27 
end
 28 
% end
28 
 
 29 
 
- 
 
 30 
figure;
29 
options = optimset('Algorithm', 'levenberg-marquardt', 'Display', 'iter-detailed', 'OutputFcn', @outfun, 'MaxIter', 100, 'TolFun', 0, 'TolX', 0);
 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 
[x, ~, ~] = lsqnonlin(@orthProcFun, xInit, [], [], options);
 33 
[x, ~, ~] = lsqnonlin(@orthProcFun, xInit, [], [], options);
31 
 
 34 
 
32 
alignment = struct('Rotation', {}, 'Translation', {});
 35 
alignment = struct('Rotation', {}, 'Translation', {});
33 
for k=1:Np
 36 
for i=1:Np
34 
    wi = x((k-1)*6+1:(k-1)*6+3);
 37 
    wi = x((i-1)*6+1:(i-1)*6+3);
35 
    Ri = axis2rmat(wi, norm(wi));
 38 
    Ri = initialAlign(i).Rotation * axis2rmat(wi, norm(wi));
36 
    Ti = x((k-1)*6+4:(k-1)*6+6);
 39 
    Ti =  initialAlign(i).Translation + x((i-1)*6+4:(i-1)*6+6);
37 
    alignment(k).Rotation = Ri;
 40 
    alignment(i).Rotation = Ri;
38 
    alignment(k).Translation = Ti;
 41 
    alignment(i).Translation =Ti;
39 
end
 42 
end
40 
 
 43 
 
41 
 
 44 
 
42 
    % objective function
 45 
    % objective function
43 
    function e = orthProcFun(x)
 46 
    function e = orthProcFun(x)
44 
 
 47 
 
45 
        Np = size(P, 1);
 - 
 
46 
        Nc = size(P, 2);
 48 
        Nc = size(P, 2);
47 
 
 49 
 
48 
        % transform all points according to current x
 50 
        % transform all points according to current x
49 
        Pbar = cell(size(P));
 51 
        Pbar = cell(size(P));
50 
        for i=1:Np
 52 
        for j=1:Np
51 
            wi = x((i-1)*6+1:(i-1)*6+3);
 53 
            wi = x((j-1)*6+1:(j-1)*6+3);
52 
            Ri = axis2rmat(wi, norm(wi));
 54 
            Ri = initialAlign(j).Rotation * axis2rmat(wi, norm(wi));
53 
            Ti = x((i-1)*6+4:(i-1)*6+6);
 55 
            Ti = initialAlign(j).Translation + x((j-1)*6+4:(j-1)*6+6);
54 
            for j=1:Nc
 56 
            for k=1:Nc
55 
                if(not(isempty(P{i,j})))
 57 
                if(not(isempty(P{j,k})))
56 
                    Pbar{i,j} = P{i,j}*Ri' + Ti';
 58 
                    Pbar{j,k} = P{j,k}*Ri' + Ti';
57 
                end
 59 
                end
58 
            end
 60 
            end
59 
        end
 61 
        end
60 
 
 62 
 
61 
        % include all pairwise distances, normalizing for each point sets
 63 
        % include all pairwise distances
62 
        e = [];
 64 
        e = [];
63 
        for i=1:Np
65 
        for j=1:Nc
64 
            % number of points in current point set
 66 
            % points in current cluster
65 
            %Npi = size(cat(1, Pbar{i,:}), 1);
 67 
            c = cat(1, Pbar{:,j});
- 
 
 68 
            Ncj = size(c, 1);
- 
 
 69
66 
            for j=1:Nc
 70 
            if(Ncj < 2)
67 
                % number of points in current cluster
 71 
                continue;
68 
                Ncj = size(cat(1, Pbar{:,j}), 1);
 72 
            end
69 
                if(not(isempty(Pbar{i,j})))
 73
70 
                    for l=setxor(1:Np, i)
 74 
            for k=1:Ncj-1
71 
                        if(not(isempty(Pbar{l,j})))
 75
72 
                            %e = [e; 1/Npi * norm(Pbar{k,j} - Pbar{i,j})];
 76 
                for l=k+1:Ncj
73 
                            e = [e; sqrt(1/Ncj) * norm(Pbar{l,j} - Pbar{i,j})];
 77
74 
                            %e = [e; norm(Pbar{k,j} - Pbar{i,j})];
 78 
                    dVec = c(k,:) - c(l,:);
75 
                        end
79
76 
                    end
80 
                    e(end+1:end+3) = dVec;
77 
                end
 81 
                end
78 
            end
 82 
            end
- 
 
 83 
 
79 
        end
 84 
        end
80 
 
 85 
 
81 
    end
 86 
    end
82 
 
 87 
 
83 
    % output function called at every iteration
 88 
    % output function called at every iteration
84 
    function stop = outfun(x, optimValues, ~)
 89 
    function stop = outfun(x, optimValues, ~)
- 
 
 90 
        %display(x);
- 
 
 91
- 
 
 92 
        hold('on');
- 
 
 93 
        plot(optimValues.residual);
- 
 
 94 
        drawnow;
85 
        stop = false;
 95 
        stop = false;
86 
    end
 96 
    end
87 
 
 97 
 
88 
end
 98 
end
89 
 
 99 
 
Line 99... Line 109...
99 
t = [R(3,2)-R(2,3),R(1,3)-R(3,1),R(2,1)-R(1,2)];
 109 
t = [R(3,2)-R(2,3),R(1,3)-R(3,1),R(2,1)-R(1,2)];
100 
 
 110 
 
101 
theta = atan2(t*w(:),trace(R(:,:))-1);
 111 
theta = atan2(t*w(:),trace(R(:,:))-1);
102 
 
 112 
 
103 
if theta<0
 113 
if theta<0
104 
    theta = -theta(1);
114 
    theta = -theta;
105 
    w(:) = -w(:);
115 
    w(:) = -w(:);
106 
end
 116 
end
107 
end
 117 
end
108 
 
 118 
 
109 
function R = axis2rmat(w, theta)
 119 
function R = axis2rmat(w, theta)
110 
 
 120 
 
111 
if(theta > 0.0001)
 - 
 
112 
    w = w./norm(w);
 - 
 
113 
end
 - 
 
114 
 
 - 
 
115 
P = w*transpose(w);
 121 
P = w*transpose(w);
116 
Q = [0 -w(3) w(2);
 122 
Q = [0 -w(3) w(2);
117 
     w(3) 0 -w(1);
 123 
     w(3) 0 -w(1);
118 
     -w(2) w(1) 0];
 124 
     -w(2) w(1) 0];
119 
 
 125