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

点群に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);

[Jiro Doke]

再現用のコードが長く処理時間もかかりそうなので、可視化の質問でしたら、RANSAC前とRANSAC後の点群のデータ（MATファイル）だけを提供して質問しなおして頂けると助かります。もし、データが大きすぎる場合は、点群の一部分だけ提供してください。

質問はなるべく簡潔に、必要最小限の情報でお願いします。

参考： Tutorial: MATLAB Answersで早く的確な回答を得るためのポイント

[michio]

上記 Jiroさんと同様、再現用のコードからは問題点を理解することが難しいので完全な推測になりますが、RANSAC前後の点群の差分を色分けして表示したいということであれば、 ismember関数 が使えるかなと。

以下のサンプルコードを実行してみてください。２次元を例にしていますが、３次元にも同じ手法が応用できるかと思います。

% 点群 xx
xx = rand(30,2);
figure(1), scatter(xx(:,1),xx(:,2),10,'r');
% 点群 yy
yy = [xx(1:10,:); rand(30,2)];
figure(2), scatter(yy(:,1),yy(:,2),10,'k');

% 点群 yy の中で 点群 xx 内に一致する点を検出
index = ismember(xx,yy,'rows');
% 一致しない点だけを抽出
yyNotInxx = yy(~index,:);
figure(3)
scatter(xx(:,1),xx(:,2),10,'r');
hold on
% 一致しない点だけを青色で表示
 scatter(yyNotInxx(:,1),yyNotInxx(:,2),10,'b');

[pokkinnko]

Jiro Dokeさん、Michioさん

ご回答ありがとうございます。 大変分かりづらかったようで申し訳ございません。 Tutorialを読み直して以後気を付けるようにします。 サンプルコードも参考にさせていただきます。

Answer 1

[Takuji Fukumoto]

MATLABでは3次元点群データの処理をComputer Vision System Toolboxでサポートしていて、 その中で二つの点群間の差分を可視化する関数があります。

xyz座標のデータとしてRANSAC処理の前後で、point_group、point_group2の変数で持っているとすると下記で色違い表示ができます。

pc1 = pointCloud(point_group)
pc2 = pointCloud(point_group2)
pcshowpair(pc1,pc2)

イメージとしては下記のような感じです。

https://jp.mathworks.com/help/vision/ug/visualize-difference-between-two-point-clouds.html?s_tid=srchtitle

[pokkinnko]

ご回答ありがとうございます。 大変助かりました。 是非参考にさせていただきます。