点群において、RAN​SAC前後の3D画像​を色分けによって一つ​の画面で比較する方法​について。

15 ビュー (過去 30 日間)
pokkinnko
pokkinnko 2017 年 1 月 4 日
コメント済み: pokkinnko 2017 年 1 月 5 日
点群に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);
end
distance=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;
end
if 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;
end
plane=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);
  3 件のコメント
pokkinnko
pokkinnko 2017 年 1 月 5 日
Jiro Dokeさん、Michioさん
ご回答ありがとうございます。 大変分かりづらかったようで申し訳ございません。 Tutorialを読み直して以後気を付けるようにします。 サンプルコードも参考にさせていただきます。

サインインしてコメントする。

採用された回答

Takuji Fukumoto
Takuji Fukumoto 2017 年 1 月 5 日
編集済み: Takuji Fukumoto 2017 年 1 月 5 日
MATLABでは3次元点群データの処理をComputer Vision System Toolboxでサポートしていて、 その中で二つの点群間の差分を可視化する関数があります。
xyz座標のデータとしてRANSAC処理の前後で、point_group、point_group2の変数で持っているとすると下記で色違い表示ができます。
pc1 = pointCloud(point_group)
pc2 = pointCloud(point_group2)
pcshowpair(pc1,pc2)
イメージとしては下記のような感じです。
  1 件のコメント
pokkinnko
pokkinnko 2017 年 1 月 5 日
ご回答ありがとうございます。 大変助かりました。 是非参考にさせていただきます。

サインインしてコメントする。

その他の回答 (0 件)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by