Subversion Repositories seema-scanner

Rev

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

Rev 218 Rev 219
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 
            Nci = size(cat(2, Pbar{i,:}), 2);
 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
- 
 
 68 
                Ncj = size(cat(1, Pbar{:,j}), 1);
67 
                if(not(isempty(Pbar{i,j})))
 69 
                if(not(isempty(Pbar{i,j})))
68 
                    for k=setxor(1:Np, i)
 70 
                    for k=setxor(1:Np, i)
69 
                        if(not(isempty(Pbar{k,j})))
 71 
                        if(not(isempty(Pbar{k,j})))
70 
                            e = [e; 1/Nci * 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})];
71 
                            %e = [e; norm(Pbar{k,j} - Pbar{i,j})];
 74 
                            %e = [e; norm(Pbar{k,j} - Pbar{i,j})];
72 
                        end
75 
                        end
73 
                    end
76 
                    end
74 
                end
 77 
                end
75 
            end
 78 
            end
Line 84... Line 87...
84 
 
 87 
 
85 
end
 88 
end
86 
 
 89 
 
87 
function [w, theta] = rmat2axis(R)
 90 
function [w, theta] = rmat2axis(R)
88 
 
 91 
 
89 
w = zeros(3, size(R,3));
 92 
w = zeros(3, 1);
90 
theta = zeros(1, size(R,3));
 93 
theta = zeros(1, 1);
91 
 
 94 
 
92 
[V,D] = eig(R);
 95 
[V,D] = eig(R);
93 
[~,ix] = min(abs(diag(D)-1));
96 
[~,ix] = min(abs(diag(D)-1));
94 
 
 97 
 
95 
w(:) = V(:,ix);
98 
w(:) = V(:,ix);
Line 114... Line 117...
114 
     w(3) 0 -w(1);
 117 
     w(3) 0 -w(1);
115 
     -w(2) w(1) 0];
 118 
     -w(2) w(1) 0];
116 
 
 119 
 
117 
% using Rodigues' rotation formula
 120 
% using Rodigues' rotation formula
118 
R = P + (eye(3) - P)*cos(theta) + Q*sin(theta);
 121 
R = P + (eye(3) - P)*cos(theta) + Q*sin(theta);
- 
 
 122 
 
- 
 
 123 
% ensure orthonormal matrix
- 
 
 124 
[U,~,V] = svd(R);
- 
 
 125 
R = U*V';
- 
 
 126 
 
119 
end
 127 
end
120 
 
 128 
 
121 
 
 129