Subversion Repositories seema-scanner

Rev

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

Rev 219 Rev 237
Line 17... Line 17...
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)
Line 60... Line 60...
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)
Line 88... Line 88...
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);