WebSVN – seema-scanner – Diff – /matlab/groupwiseOrthogonalProcrustes.m


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
 
 
 

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

Subversion Repositories seema-scanner

(root)/matlab/groupwiseOrthogonalProcrustes.m – Rev 219 → 237