1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49
| % Detection d'un cylindre
clear all;
close all;
% Charger les points
[X1]= stlread('cylinder.stl');
X=unique(X1,'rows');
%Initialisation
gamma = 0.01;
L=[];
Lmeilleur=[];
for it=1:100
i=1;
% pick up 3 random points
M= X(floor(rand(3,1)*(size(X,1)))+1,:);
%Identifaction of plan=>three points (xi, yi, zi), which are not collinear, are necessary: a*xi + b*yi + c*zi +d
V1 = transpose(M(1,:)- M(2,:));
V2 = transpose(M(1,:)- M(3,:));
N = cross(V1,V2);
d = (N(1)*M(1,1)+N(2)*M(1,2)+N(3)*M(1,3));
% the radius (r) and the center c = (x0, y0, z0) are obtained by solving the following equations.
syms x0 y0 z0 r;
eq=[((M(1,1)-x0)^2)+((M(1,2)-y0)^2)+((M(1,3)-z0)^2)==r^2;
((M(2,1)-x0)^2)+((M(2,2)-y0)^2)+((M(2,3)-z0)^2)==r^2;
((M(3,1)-x0)^2)+((M(3,2)-y0)^2)+((M(3,3)-z0)^2==r^2);
(N(1)*x0)+(N(2)*y0)+(N(3)*z0)-d==0];
sol=solve(eq,[x0,y0,z0,r],'IgnoreAnalyticConstraints',true,'PrincipalValue', true);
x0 = vpa(sol.x0); y0 = vpa(sol.y0);z0 = vpa(sol.z0);r = vpa(sol.r);
C=[x0 y0 z0];
% the minimum distance (dminj ) between each point of D and the cylinder axis
for ind= 1:length(X)
P = transpose(C-X(ind,:));
D = norm(cross(N,P))/norm(N);
if D < r+gamma
L(i,:)=X(ind,:);
i=i+1;
end
end
if length(L)>length(Lmeilleur)
Lmeilleur=L;
end
end
% Plot
figure; hold on;
plot3(X(:,1),X(:,2),X(:,3),'or');
plot3(Mmeilleur(:,1),Mmeilleur(:,2),Mmeilleur(:,3),'og');
xlabel('x');ylabel('y');zlabel('z');
title('RANSAC results for cylinder estimation');
legend('Outliers','Inliers');
axis equal tight; |
Partager