点群にRANSACを行いました。RANSAC前後の2つの3D画像を比較するため、RANSAC前の点群とRANSAC後の点群を重ね合わせ、RANSACによって除去された点の部分だけ色を変えたいです。色はなんでも構いません。このようなコードはありますか? 以下に、使ったデータとRANSACの手順を記載します。 なお、コードの154列目以降「point_group2が定義されていません」とエラーが出る可能性があります。 その場合148列まで実行しpoint_groupを複製してpoint_group2と名前をつけ、ワークスペースに加えた後、155列目から実行してください。
xyz_T2_1highreso_kyoukyaku2 = horzcat(T2_1highreso_kyoukyaku2.X,T2_1highreso_kyoukyaku2.Y,T2_1highreso_kyoukyaku2.Z);X = xyz_T2_1highreso_kyoukyaku2(:,1);Y = xyz_T2_1highreso_kyoukyaku2(:,2);Z = xyz_T2_1highreso_kyoukyaku2(:,3);dat_XYZ = xyz_T2_1highreso_kyoukyaku2;n=1;plane=zeros(1,5);plane_point=zeros(1,3);point_num0=length(dat_XYZ(:,1));disp('RANSACによる面検出...');point_group=dat_XYZ(1:point_num0(1,1),:);A=0;while A<=300000 S=1000; if ((A/1000)==ceil(A/1000)) fprintf('[%d/%d] %0.2f[sec]\n',A,300000,tic); end %点群数確認
point_num=length(point_group(:,1)); if point_num<=150 break end if A>290000 if point_num>2000 S=800; end end %ランダムに抽出した面と各点の距離を算出
%点群データを一部を切り出し
%ランダムに3点抽出
code_1=randi([1,point_num],3,1); point_A=zeros(1,3); point_B=zeros(1,3); point_C=zeros(1,3); %3点のx,y,z座標
point_A(1,1)=point_group(code_1(1,1),1); point_A(1,2)=point_group(code_1(1,1),2); point_A(1,3)=point_group(code_1(1,1),3);point_B(1,1)=point_group(code_1(2,1),1);point_B(1,2)=point_group(code_1(2,1),2);point_B(1,3)=point_group(code_1(2,1),3);point_C(1,1)=point_group(code_1(3,1),1);point_C(1,2)=point_group(code_1(3,1),2);point_C(1,3)=point_group(code_1(3,1),3); %抽出した3点を通る平面の方程式(外積を利用)
AB_vector=(point_B-point_A); AC_vector=(point_C-point_A);na=(AB_vector(1,2)*AC_vector(1,3)-AB_vector(1,3)*AC_vector(1,2));nb=(AB_vector(1,3)*AC_vector(1,1)-AB_vector(1,1)*AC_vector(1,3));nc=(AB_vector(1,1)*AC_vector(1,2)-AB_vector(1,2)*AC_vector(1,1));d0=-(na*point_A(1,1)+nb*point_A(1,2)+nc*point_A(1,3)); %各面と面の距離の算出
i=1000; rand_point=randi([1,point_num],i,1); code_2=zeros(i,3); distance_A=zeros(i,1); s=zeros(1,1);for k=1:i code_2(k,1)=point_group(rand_point(k,1),1); code_2(k,2)=point_group(rand_point(k,1),2); code_2(k,3)=point_group(rand_point(k,1),3); x0=code_2(k,1); y0=code_2(k,2); z0=code_2(k,3); distance_A(k,1)=abs(na*x0+nb*y0+nc*z0+d0)/sqrt(na^2+nb^2+nc^2); enddistance=distance_A.*distance_A;distance_sum=sum(distance);threshold=distance_sum/i;s(n,1)=threshold;Med=median(s);m=0.0040; %閾値(点群によって値を変更)
L=length(find(distance_A<=m)); %距離がm以下の点の総数
cos=nc/sqrt(na^2+nb^2+nc^2);%面を検出
if cos<=0.02 if S<=L S=S+L; Lmax=max(L); if L<=Lmax; a=na; b=nb; c=nc; d=d0; %検出した面と全ての点の距離を算出
plane(n,:)=zeros(1,5); plane(n,1)=L; plane(n,2)=a; plane(n,3)=b; plane(n,4)=c; plane(n,5)=d; distance_B=zeros(point_num,3); reverce_B=distance_B; for k=1:point_num xx=point_group(k,1); yy=point_group(k,2); zz=point_group(k,3); distance_B(k,1)=abs(a*xx+b*yy+c*zz+d)/sqrt(a^2+b^2+c^2); end %検出した面の点群を除外して別の場所に格納
distance_B(:,2)=distance_B(:,1); distance_B(:,3)=distance_B(:,1);if m<1 %閾値m=1未満の場合に使用
reverce_B(abs(distance_B)>m)=m+1; distance_B(abs(distance_B)>m)=1; reverce_B(abs(distance_B)<=m)=1; distance_B(abs(distance_B)<=m)=0; reverce_B(reverce_B==m+1)=0;endif m>=1 %閾値m=1,それ以上の場合に使用
reverce_B(distance_B<=m)=1; distance_B(distance_B<=m)=0; reverce_B(distance_B>m)=0; distance_B(distance_B>m)=1;end%point_group2が最初に出てくる場所
%point_group2が定義されていないとエラーが出たときは
%ワークスペースでpoint_groupを複製してpoint_group2と名付けると良い
point_group2 = point_group; %point_group2は面で検出した点の一時格納領域
point_group = sortrows(point_group .* distance_B); point_group2 = sortrows(point_group2 .* reverce_B); for k=1:point_num if abs(point_group(k,1)+point_group(k,2)+point_group(k,3))==0 break end end for kk=k:point_num if abs(point_group(kk,1)+point_group(kk,2)+point_group(kk,3))>0 break end end if k~=point_num point_group(k:kk-1,:)=[]; end plane_num=length(point_group2(:,1)); point_group2=sortrows(point_group2); for k=1:plane_num if abs(point_group2(k,1)+point_group2(k,2)+point_group2(k,3))==0 break end end for kk=k:plane_num if abs(point_group2(kk,1)+point_group2(kk,2)+point_group2(kk,3))>0 break end end if k~=plane_num point_group2(k:kk-1,:)=[]; end LL=length(point_group2); point_group2(LL,:)=[]; if point_group2 ~= 0 point_group2(1,:) = []; plane_point=[plane_point;point_group2]; end n=n+1; end end end A=A+1; endplane=sortrows(plane);plane_point(1,:)=[]; X=point_group2(:,1); Y=point_group2(:,2); Z=point_group2(:,3); %確認表示(RANSAC処理)
figure; plot3(X,Y,Z,'b.','MarkerSize',0.1); view(0,90); %確認表示(平面以外)
LLL=length(point_group); point_group(LLL,:)=[]; % figure;
% plot3(point_group(:,1),point_group(:,2),point_group(:,3),'b.','MarkerSize',0.1);
% view(0,90);
%平面より上にあるか下にあるか分類
disp('平面より上か下かの分類...');flat=unique(plane_point(:,3));f=length(flat);C=zeros(length(point_group),f);for j=1:length(flat(:,1)); C(:,j)=point_group(:,3)-flat(j,1);end CC=mean(C,2); %平面より下の部分
below=find(CC<0); below_XYZ(below,1)=point_group(below,1); below_XYZ(below,2)=point_group(below,2); below_XYZ(below,3)=point_group(below,3); below_zero=find(below_XYZ(:,1)==0); below_XYZ(below_zero,:)=[]; %確認表示(平面より下)
% figure;
% plot3(below_XYZ(:,1),below_XYZ(:,2),below_XYZ(:,3),'b.','MarkerSize',0.1);
% view(0,90);
%平面より上の部分
above=find(CC>=0); above_XYZ(above,1)=point_group(above,1); above_XYZ(above,2)=point_group(above,2); above_XYZ(above,3)=point_group(above,3); above_zero=find(above_XYZ(:,1)==0); above_XYZ(above_zero,:)=[]; %確認表示(平面より上)
% figure; % plot3(above_XYZ(:,1),above_XYZ(:,2),above_XYZ(:,3),'b.','MarkerSize',0.1);
% view(0,90);
Best Answer