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

# この記事に書いてあること
Open3D（Python版）のコードはMATLABでどのように書くのかをOpen3Dの[Tutorial](https://www.open3d.org/docs/release/tutorial/geometry/pointcloud.html)をベースに紹介していきます。2つの環境を使っている（もしくは使う予定のある）エンジニアが、「どう書くんだっけ？」と思った際の参考になればと思います。
各関数の入力引数について詳細は記載しないので、適宜ドキュメントをあたっていただければと思います。
# Version
Open3D：0.18.0
MATLAB：R2024b Prerelease　（主にComputer Vision ToolboxとLidar Toolbox）
# 点群データの読込みと表示
## Open3D
必要パッケージのインポート

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


点群（PLYファイル）の読込み
```python
ply_point_cloud = o3d.data.PLYPointCloud() # デモデータの読込み
pcd = o3d.io.read_point_cloud(ply_point_cloud.path) # PLYファイルの読込み 
o3d.visualization.draw_geometries([pcd]) # 表示
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/f132c602-3999-7597-b04b-a510c2084096.png)

データのアクセス
実体をハンドリングするためにはnp.asarrayで数値配列に変換する必要がある。
```python
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はあとで使います。
```python
pcd_point_cloud = o3d.data.PCDPointCloud()
pcd2 = o3d.io.read_point_cloud(pcd_point_cloud.path)
o3d.visualization.draw_geometries([pcd2])
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/dbadc54d-d33a-4122-deb4-ed54fce1b7cc.png)

メッシュファイルの読込み
```python
bunny = o3d.data.BunnyMesh()
mesh = o3d.io.read_triangle_mesh(bunny.path)
o3d.visualization.draw_geometries([mesh])
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/6ef50f24-ccc2-b368-ab54-1d7f45ce3912.png)

## MATLAB
点群の読込み
```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

```

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/69bca717-7ddc-75da-e249-e5809be9d5f2.png)

メッシュファイルの読込み
メッシュファイルの読込みは[readSurfaceMesh](https://jp.mathworks.com/help/lidar/ref/readsurfacemesh.html)関数を使う。FBXなども読み込める。
```matlab
mesh = readSurfaceMesh('bunny.stl');
mesh
surfaceMeshShow(mesh);
```
実行結果
```matlab
mesh = 

  surfaceMesh with properties:

         Vertices: [43318×3 double]
            Faces: [86632×3 int32]
    VertexNormals: []
     VertexColors: []
      FaceNormals: [86632×3 double]
       FaceColors: []
      NumVertices: 43318
         NumFaces: 86632
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/bb6f5e44-bdb1-06a4-dc75-d1a4d4f0f33d.png)

# ROI抽出（ポリゴン）
## Open3D
JSONファイルを読み込んで種々の設定を反映する。
```python
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で指定するようです。  

出力結果
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/cfdb557c-7d4e-d1c5-39bf-12d0e5356403.png)

## MATLAB
MATLABに同じ機能の関数はないですが、単純な直方体や円筒形のROIの場合、pointCloudオブジェクトの[findPointsInROI](https://jp.mathworks.com/help/releases/R2024b/vision/ref/pointcloud.findpointsinroi.html)と[findPointsInCylinder](https://jp.mathworks.com/help/releases/R2024b/vision/ref/pointcloud.findpointsincylinder.html)関数が使えます。

```MATLAB
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)
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/82824569-1e69-56a3-53a8-baaa200fa013.png)

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

```MATLAB
% ポリゴンの範囲を指定
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)
```
出力結果
ポリゴン選択
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/a1b96d3c-3a93-8c06-079a-05748e80b41c.png)

選択後
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/8944d421-2eef-2e37-c2b5-7deda2a93b41.png)
</details>

# ダウンサンプリング
グリッドの中かから1点選択するやつです。
## Open3D
```Python
downpcd = pcd.voxel_down_sample(voxel_size=0.1)
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/951e5c2d-abb9-4515-8d1b-5567375f7775.png)

## MATLAB
```MATLAB
downpcd = pcdownsample(pcd,'gridAverage',0.1);
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/56288d48-08fc-8e77-68c6-4b3fd8fbd4e1.png)
ランダムサンプリングなどもできる（[pcdownsample](https://jp.mathworks.com/help/vision/ref/pcdownsample.html)）。
# 法線推定
Open3Dは主成分分解ベースの法線推定アルゴリズム、MATLABの方は平面近似がベースのアルゴリズムになっているため、出てくる値は異なります。
## Open3D
```python
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行
```
実行結果
```Python
[ 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
```MATLAB
downpcd.Normal = pcnormals(downpcd,30); % 近傍の30点を平面近似
downpcd.Normal(1,:)
downpcd.Normal(1:10,:)
```
実行結果
```MATLAB
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
```python
chair.paint_uniform_color([1, 0.706, 0])
o3d.visualization.draw_geometries([chair])
```
出力結果
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/90267e56-98e2-e595-524d-baf68ab7f5fb.png)

## MATLAB
```MATLAB
pcd.Color = repmat([1 0.706 0],pcd.Count,1);
pcshow(pcd,'ColorSource','Color');
```
出力結果
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/33f3d140-7451-152f-8fc7-90b840920026.png)

# 点群間の距離
## Open3D
```Python
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])
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/12819a64-3afe-8570-9b1e-a72d347f5d27.png)

## MATLAB
```matlab
[indices, dists] = pcdROI.multiQueryKNNSearchImpl(pcd.Location,1);
distIdx = dists'>0.01; % 行ベクトルに変換する
pcdROIout = pcd.select(distIdx);
pcshow(pcdROIout);
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/2ba73008-3929-cd2d-692f-dfddb9f3aea0.png)

# 境界ボックス
## Open3D
軸に沿ったボックスフィッティングと、3次元的なフィッティングの両方をサポートしている。
```Python
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])
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/d2703347-1a6d-8b8d-91b0-1dcce0e327da.png)

## MATLAB
yaw角だけ考慮した直方体フィッティングか関数として利用可能。軸に沿った直方体は手計算となる。
```matlab
% 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;
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/bfe4ae1b-ecc0-79b2-407b-f6e3b50b3931.png)

# 点群の凸包
## Open3D
```Python
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])
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/875c860e-139f-744d-8e57-747a5c56e03b.png)

## MATLAB
```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)
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/38df585c-0a91-4b04-20b3-4a29c063ac16.png)

# クラスタリング
## Open3D
DBSCANベース
```Python
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],
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/7b53ede8-5265-dc43-8c59-60048e2295b5.png)

## MATLAB
DBSCANベースの方法はstatistics and machine learning Toolboxにありますが、点群処理として標準的に用いるToolboxであるComputer Vision ToolboxおよびLidar Toolboxでは、距離ベースの手法でのクラスタリングアルゴリズムがサポートされています。
```matlab
[labels,numClusters] = pcsegdist(pcdROIout,0.1);
pcshow(pcdROIout.Location,labels)
colormap(hsv(numClusters))
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/0f0a5790-954c-3d18-17d6-57a52d8f65bc.png)

# 平面のセグメンテーション
## Open3D
```Python
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])
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/c563e33f-a067-c835-a65a-bed37f23d5f6.png)

## MATLAB
```matlab
load object3d.mat
[mdl,inlier,outlier] = pcfitplane(ptCloud,0.01);
pcInlier = ptCloud.select(inlier);
pcshow(ptCloud);
hold on;
pcshow(pcInlier.Location,'r')
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/da0dac72-ac38-48c7-14a8-2b63618e18d6.png)

# 平面パッチ検出
## Open3D
```Python
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)
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/529ef770-691d-fb61-0783-4201c1888ec7.png)

## MATLAB
```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;
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/222bcf74-10c7-3b1d-33f0-051341727476.png)

# 隠点の除去
## Open3D
```Python
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])
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/b8bddd3c-9ca4-3754-b07a-68ae011bb578.png)

## MATLAB
```matlab
pcd_removed = removeHiddenPoints(pcd,[0,0,10]); % 視点を入力
pcshow(pcd_removed);
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/202697/f667f78c-b83a-8ec6-b3e8-e4a1c39919de.png)

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