【MATLABとOpen3D（Python）】点群処理コードの対応

この記事に書いてあること

Open3D（Python版）のコードはMATLABでどのように書くのかをOpen3DのTutorialをベースに紹介していきます。2つの環境を使っている（もしくは使う予定のある）エンジニアが、「どう書くんだっけ？」と思った際の参考になればと思います。
各関数の入力引数について詳細は記載しないので、適宜ドキュメントをあたっていただければと思います。

Version

Open3D：0.18.0
MATLAB：R2024b Prerelease　（主にComputer Vision ToolboxとLidar Toolbox）

点群データの読込みと表示

Open3D

必要パッケージのインポート

import open3d as o3d
import numpy as np
import matplotlib.pyplot as plt

点群（PLYファイル）の読込み

ply_point_cloud = o3d.data.PLYPointCloud() # デモデータの読込み
pcd = o3d.io.read_point_cloud(ply_point_cloud.path) # PLYファイルの読込み 
o3d.visualization.draw_geometries([pcd]) # 表示

データのアクセス
実体をハンドリングするためにはnp.asarrayで数値配列に変換する必要がある。

print(pcd)
print(np.asarray(pcd.points))

実行結果

PointCloud with 196133 points.
[[0.65234375 0.84686458 2.37890625]
 [0.65234375 0.83984375 2.38430572]
 [0.66737998 0.83984375 2.37890625]
 ...
 [2.00839925 2.39453125 1.88671875]
 [2.00390625 2.39488506 1.88671875]
 [2.00390625 2.39453125 1.88793314]]

点群（PCDファイル）の読込み
PLYと一緒です。このpcd2はあとで使います。

pcd_point_cloud = o3d.data.PCDPointCloud()
pcd2 = o3d.io.read_point_cloud(pcd_point_cloud.path)
o3d.visualization.draw_geometries([pcd2])

メッシュファイルの読込み

bunny = o3d.data.BunnyMesh()
mesh = o3d.io.read_triangle_mesh(bunny.path)
o3d.visualization.draw_geometries([mesh])

MATLAB

点群の読込み

pcd = pcread('teapot.ply'); % 点群読込み
pcd % pointCloudオブジェクトの中身を表示
pcd.Location(1:10,:) % 最初の10行の座標数値情報を表示
pcshow(pcd); % 表示

実行結果

pcd = 

  pointCloud with properties:

     Location: [41472×3 single]
        Count: 41472
      XLimits: [-3 3.4340]
      YLimits: [-2 2]
      ZLimits: [0 3.1500]
        Color: []
       Normal: []
    Intensity: []

ans =

  10×3 single matrix

         0   -1.5000    2.4000
         0   -1.4946    2.4109
         0   -1.4890    2.4212
         0   -1.4832    2.4309
         0   -1.4774    2.4399
         0   -1.4715    2.4482
         0   -1.4656    2.4559
         0   -1.4596    2.4630
         0   -1.4537    2.4694
         0   -1.4477    2.4752

メッシュファイルの読込み
メッシュファイルの読込みはreadSurfaceMesh関数を使う。FBXなども読み込める。

mesh = readSurfaceMesh('bunny.stl');
mesh
surfaceMeshShow(mesh);

実行結果

mesh = 

  surfaceMesh with properties:

         Vertices: [43318×3 double]
            Faces: [86632×3 int32]
    VertexNormals: []
     VertexColors: []
      FaceNormals: [86632×3 double]
       FaceColors: []
      NumVertices: 43318
         NumFaces: 86632

ROI抽出（ポリゴン）

Open3D

JSONファイルを読み込んで種々の設定を反映する。

demo_crop_data = o3d.data.DemoCropPointCloud()
print(demo_crop_data)
pcd = o3d.io.read_point_cloud(demo_crop_data.point_cloud_path)
vol = o3d.visualization.read_selection_polygon_volume(demo_crop_data.cropped_json_path)
chair = vol.crop_point_cloud(pcd)
o3d.visualization.draw_geometries([chair])

このデモのJSONファイルは下記のようになっていました。

{
	"axis_max" : 4.022921085357666,
	"axis_min" : -0.76341366767883301,
	"bounding_polygon" : 
	[
		[ 2.6509309513852526, 0.0, 1.6834473132326844 ],
		[ 2.5786428246917148, 0.0, 1.6892074266735244 ],
		[ 2.4625790337552154, 0.0, 1.6665777078297999 ],
		[ 2.2228544982251655, 0.0, 1.6168160446813649 ],
		[ 2.166993206001413, 0.0, 1.6115495157201662 ],
		[ 2.1167895865303286, 0.0, 1.6257706054969348 ],
		[ 2.0634657721747383, 0.0, 1.623021658624539 ],
		[ 2.0568612343437236, 0.0, 1.5853892911207643 ],
		[ 2.1605399001237027, 0.0, 0.96228993255083017 ],
		[ 2.1956669387205228, 0.0, 0.95572746049785073 ],
		[ 2.2191318790575583, 0.0, 0.88734449982108754 ],
		[ 2.2484881847925919, 0.0, 0.87042807267013633 ],
		[ 2.6891234157295827, 0.0, 0.94140677988967603 ],
		[ 2.7328692490470647, 0.0, 0.98775740674840251 ],
		[ 2.7129337547575547, 0.0, 1.0398850034649203 ],
		[ 2.7592174072415405, 0.0, 1.0692940558509485 ],
		[ 2.7689216419453428, 0.0, 1.0953914441371593 ],
		[ 2.6851455625455669, 0.0, 1.6307334122162018 ],
		[ 2.6714776099981239, 0.0, 1.675524657088997 ],
		[ 2.6579576128816544, 0.0, 1.6819127849749496 ]
	],
	"class_name" : "SelectionPolygonVolume",
	"orthogonal_axis" : "Y",
	"version_major" : 1,
	"version_minor" : 0
}

ポリゴンはX,Y,Zの内2軸で定義される平面で定義されるようです（違ったら誰か教えてください）。平面の法線となる軸をorthogonal_axisで指定して、orthogonal_axis上のROIはaxis_maxとaxis_minで指定するようです。

出力結果

MATLAB

MATLABに同じ機能の関数はないですが、単純な直方体や円筒形のROIの場合、pointCloudオブジェクトのfindPointsInROIとfindPointsInCylinder関数が使えます。

roi = [-1.58 1.8855 -2 2 0 2.54]; %[xmin xmax ymin ymax zmin zmax]
roiIdx = pcd.findPointsInROI(roi);
pcdROI = pcd.select(roiIdx);
pcshow(pcdROI)

念のためOpen3Dのようなポリゴンで範囲抽出ソースコードも載せておきます。

% ポリゴンの範囲を指定
orthogonal_axis = "Y";
[axis_min,axis_max] = deal(-inf,inf); % orthogonal_axisの最小・最大値
xyz = pcd.Location; % 3次元座標値
d = dictionary(["X","Y","Z"],[1 2 3]); 
proj_axis = setxor([1 2 3],d(orthogonal_axis)); % 投影面の軸を抽出
proj_points = xyz(:,proj_axis); % 投影面における座標
plot(proj_points(:,1),proj_points(:,2),'r.');
h = drawpolygon; % ポリゴンを指定
poly = h.Position;
% ポリゴン範囲の抽出
inPolyIndx = inpolygon(proj_points(:,1),proj_points(:,2),poly(:,1),poly(:,2)); % ポリゴンの範囲
inOrthoIdx = xyz(:,d(orthogonal_axis)) >= axis_min & xyz(:,d(orthogonal_axis)) <= axis_max; % orthogonal_axisの範囲
inIdx = inPolyIndx & inOrthoIdx; 
pcSelected = pcd.select(inIdx); % 範囲内点群を選択
pcshow(pcSelected)

出力結果
ポリゴン選択

選択後

ダウンサンプリング

グリッドの中かから1点選択するやつです。

Open3D

downpcd = pcd.voxel_down_sample(voxel_size=0.1)

MATLAB

downpcd = pcdownsample(pcd,'gridAverage',0.1);

ランダムサンプリングなどもできる（pcdownsample）。

法線推定

Open3Dは主成分分解ベースの法線推定アルゴリズム、MATLABの方は平面近似がベースのアルゴリズムになっているため、出てくる値は異なります。

Open3D

downpcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) # 主成分分析ベース
print(downpcd.normals[0]) # 法線の1行目
print(np.asarray(downpcd.normals)[:10, :]) # 上から10行

実行結果

[ 0.00348136 -0.09321477 -0.99563994]
[[ 0.00348136 -0.09321477 -0.99563994]
 [-0.02348153  0.20308955  0.97887857]
 [ 0.98798252 -0.06762201 -0.13898847]
 [ 0.00864165 -0.05437875 -0.99848299]
 [ 0.00864159 -0.05437876 -0.99848299]
 [ 0.01261311 -0.72752996 -0.68595995]
 [ 0.00175451 -0.0256587  -0.99966922]
 [ 0.          0.          1.        ]
 [ 0.          0.          1.        ]
 [-0.01916377  0.13835191  0.99019771]]

MATLAB

downpcd.Normal = pcnormals(downpcd,30); % 近傍の30点を平面近似
downpcd.Normal(1,:)
downpcd.Normal(1:10,:)

実行結果

ans =

  1×3 single row vector

   -0.1595   -0.5931   -0.7892


ans =

  10×3 single matrix

   -0.1595   -0.5931   -0.7892
    0.1211    0.5680    0.8141
    0.0749    0.5508    0.8313
    0.0231    0.5488    0.8356
    0.0325    0.5447    0.8380
   -0.0063    0.5462    0.8376
   -0.0368    0.5327    0.8455
   -0.0682    0.5525    0.8307
   -0.1102    0.5289    0.8415
    0.1595   -0.5931   -0.7892

色の変更

Open3D

chair.paint_uniform_color([1, 0.706, 0])
o3d.visualization.draw_geometries([chair])

出力結果

MATLAB

pcd.Color = repmat([1 0.706 0],pcd.Count,1);
pcshow(pcd,'ColorSource','Color');

出力結果

点群間の距離

Open3D

dists = pcd.compute_point_cloud_distance(chair)
dists = np.asarray(dists)
ind = np.where(dists > 0.01)[0]
pcd_without_chair = pcd.select_by_index(ind)
o3d.visualization.draw_geometries([pcd_without_chair])

MATLAB

[indices, dists] = pcdROI.multiQueryKNNSearchImpl(pcd.Location,1);
distIdx = dists'>0.01; % 行ベクトルに変換する
pcdROIout = pcd.select(distIdx);
pcshow(pcdROIout);

境界ボックス

Open3D

軸に沿ったボックスフィッティングと、3次元的なフィッティングの両方をサポートしている。

aabb = chair.get_axis_aligned_bounding_box()
aabb.color = (1, 0, 0)
obb = chair.get_oriented_bounding_box()
obb.color = (0, 1, 0)
o3d.visualization.draw_geometries([chair, aabb, obb])

MATLAB

yaw角だけ考慮した直方体フィッティングか関数として利用可能。軸に沿った直方体は手計算となる。

% yaw角のみ考慮したフィッティング
mdl = pcfitcuboid(pcd);
pcshow(pcd)
hold on;
h1 = mdl.plot;
% 軸に合わせたフィッティング
limits = [pcd.XLimits;pcd.YLimits;pcd.ZLimits];
centers = mean(limits,2);
sizes = diff(limits,1,2);
mdlAxes = cuboidModel([centers',sizes',0 0 0]);
h2 = mdlAxes.plot;

点群の凸包

Open3D

pcl = mesh.sample_points_poisson_disk(number_of_points=2000)
hull, _ = pcl.compute_convex_hull()
hull_ls = o3d.geometry.LineSet.create_from_triangle_mesh(hull)
hull_ls.paint_uniform_color((1, 0, 0))
o3d.visualization.draw_geometries([pcl, hull_ls])

MATLAB

DT = delaunayTriangulation(mesh.Vertices);
[C,v] = DT.convexHull;
pcshow(mesh.Vertices,'MarkerSize',10);
hold on;
trisurf(C,DT.Points(:,1),DT.Points(:,2),DT.Points(:,3), ...
       'FaceColor','cyan','FaceAlpha',0.5)

クラスタリング

Open3D

DBSCANベース

with o3d.utility.VerbosityContextManager(
        o3d.utility.VerbosityLevel.Debug) as cm:
    labels = np.array(
        pcd.cluster_dbscan(eps=0.02, min_points=10, print_progress=True))

max_label = labels.max()
print(f"point cloud has {max_label + 1} clusters")
colors = plt.get_cmap("tab20")(labels / (max_label if max_label > 0 else 1))
colors[labels < 0] = 0
pcd.colors = o3d.utility.Vector3dVector(colors[:, :3])
o3d.visualization.draw_geometries([pcd],

MATLAB

DBSCANベースの方法はstatistics and machine learning Toolboxにありますが、点群処理として標準的に用いるToolboxであるComputer Vision ToolboxおよびLidar Toolboxでは、距離ベースの手法でのクラスタリングアルゴリズムがサポートされています。

[labels,numClusters] = pcsegdist(pcdROIout,0.1);
pcshow(pcdROIout.Location,labels)
colormap(hsv(numClusters))

平面のセグメンテーション

Open3D

plane_model, inliers = pcd2.segment_plane(distance_threshold=0.01,
                                         ransac_n=3,
                                         num_iterations=1000)
[a, b, c, d] = plane_model
print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")

inlier_cloud = pcd2.select_by_index(inliers)
inlier_cloud.paint_uniform_color([1.0, 0, 0])
outlier_cloud = pcd2.select_by_index(inliers, invert=True)
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud])

MATLAB

load object3d.mat
[mdl,inlier,outlier] = pcfitplane(ptCloud,0.01);
pcInlier = ptCloud.select(inlier);
pcshow(ptCloud);
hold on;
pcshow(pcInlier.Location,'r')

平面パッチ検出

Open3D

oboxes = pcd2.detect_planar_patches(
    normal_variance_threshold_deg=60,
    coplanarity_deg=75,
    outlier_ratio=0.75,
    min_plane_edge_length=0,
    min_num_points=0,
    search_param=o3d.geometry.KDTreeSearchParamKNN(knn=30))

print("Detected {} patches".format(len(oboxes)))

geometries = []
for obox in oboxes:
    mesh = o3d.geometry.TriangleMesh.create_from_oriented_bounding_box(obox, scale=[1, 1, 0.0001])
    mesh.paint_uniform_color(obox.color)
    geometries.append(mesh)
    geometries.append(obox)
geometries.append(pcd2)

o3d.visualization.draw_geometries(geometries)

MATLAB

load object3d.mat
ptCloud.Normal = pcnormals(ptCloud,30); % 法線推定が前提
[labels,numPlanes,varargout] = pcsegplanes(ptCloud,"MinNumPoints",3000,"MinDimensionSize",0.3);
pcshow(ptCloud);
hold on;
for i = 1:numPlanes
    ptCloud_this = ptCloud.select(labels==i);
    pcshow(ptCloud_this.Location,'r','MarkerSize',20);
end
hold off;

隠点の除去

Open3D

pcd3 = mesh.sample_points_poisson_disk(5000)
diameter = np.linalg.norm(
    np.asarray(pcd3.get_max_bound()) - np.asarray(pcd3.get_min_bound()))
o3d.visualization.draw_geometries([pcd3])
print("Define parameters used for hidden_point_removal")
camera = [0, 0, diameter]
radius = diameter * 100

print("Get all points that are visible from given view point")
_, pt_map = pcd3.hidden_point_removal(camera, radius)

print("Visualize result")
pcd3 = pcd3.select_by_index(pt_map)
o3d.visualization.draw_geometries([pcd3])

MATLAB

pcd_removed = removeHiddenPoints(pcd,[0,0,10]); % 視点を入力
pcshow(pcd_removed);

まとめ

Open3D（Python）とMATLABの点群処理系のコードの対応を調べました。対応するものがないものもありましたが、他の関数で書けたりおよそ代替する関数があります。うまく使いこなしていきたいですね。