RealSense D435iで3Dスキャナもどきの実装

# Depthカメラで3Dスキャナもどきの実装（Open3Dの探索をかねて。）
前回に続いてRealSense D435iで遊んでいきます。

## 概要
* D435iでRGBDデータを作成する
* RGBDデータから点群データを生成する
* 点群データから法線を自動計算して、Meshを貼る

## 環境
* MacBook Air
* Intel RealSense D435i
* Jupyter-notebook
* Python
* Open3D

## 参考
* Open3Dドキュメント
http://www.open3d.org/docs/release/
* 前回記事
https://qiita.com/tishihara/items/90ce0f0f722deddea34a
* メッシュ作成アルゴリズム
https://stackoverflow.com/questions/56965268/how-do-i-convert-a-3d-point-cloud-ply-into-a-mesh-with-faces-and-vertices

## 手順

* ライブラリのインポート

realsenseのSDKとopen3dをインポートします。

```python
import pyrealsense2 as rs
import numpy as np
import open3d as o3d
```

* 画角を合わせつつ、ストリーミング情報を取得する

RGBDを作成するためには、DepthとRGBの画角をあわせる必要があります。
画角をあわせるためにはalignオブジェクトを使います。SDKにこういったメソッドがあるのは便利ですね。

```python
# ストリーム(Depth/Color)の設定
config = rs.config()
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)

# ストリーミング開始
pipeline = rs.pipeline()
profile = pipeline.start(config)

# Alignオブジェクト生成
align_to = rs.stream.color
align = rs.align(align_to)
```
* 画角が同じRGB画像とDepth画像の２つから、RGBD画像を生成

RGBD画像作成のメソッドは`create_from_color_and_depth`です。

```python
try:
    while True:
        # フレーム待ち(Color & Depth)
        frames = pipeline.wait_for_frames()
        aligned_frames = align.process(frames)
        color_frame = aligned_frames.get_color_frame()
        depth_frame = aligned_frames.get_depth_frame()
        if not depth_frame or not color_frame:
            continue
            
        color_image = o3d.geometry.Image(np.asanyarray(color_frame.get_data()))
        depth_image = o3d.geometry.Image(np.asanyarray(depth_frame.get_data()))
        
        rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(color_image, depth_image);
```

* RGBD画像からPoint Cloudを生成

RGBD画像があれば`PointCloud.create_from_rgbd_image`を使って一発でPoint Cloudを作成できます。

```python
        pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image,
                                                             o3d.camera.PinholeCameraIntrinsic(o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault))
```

※`PrimeSenseDefault`を指定していますが、本来ここはRealSense D435iでカメラキャリブレーションを行い、内部パラメータを求めた上で、内部パラメータ(instric)の行列を与えるべきかと思います。今回はキャリブレーションまで行えてないので、仮で上記を設定しました。

* 回転、法線計算、ダウンサンプリングなどの処理

メッシュを貼る前に、点群が細かすぎると表示がいまいちなので、ダウンサンプリングします。

```python
        # 回転する
        pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
        
        # 法線計算
        pcd.estimate_normals()
        
        # 指定したvoxelサイズでダウンサンプリング
        voxel_down_pcd = pcd.voxel_down_sample(voxel_size=0.01)
```

* メッシュを貼る

すみません、このあたりには明るくないです。
ドキュメントをみていたら`create_from_point_cloud_ball_pivoting`というメソッドがあったので、これを使ってみたらPCDからメッシュデータを生成することができました。
一部メッシュが貼られないのは、半径の設定の問題なのでしょうか？
アルゴリズムについてはこちらに記載があります。
https://ieeexplore.ieee.org/document/817351

```python        
        distances = voxel_down_pcd.compute_nearest_neighbor_distance()
        avg_dist = np.mean(distances)
        radius = 1.5 * avg_dist
        
        # メッシュ化
        radii = [radius, radius * 2]
        mesh = o3d.geometry.TriangleMesh.create_from_point_cloud_ball_pivoting(voxel_down_pcd, o3d.utility.DoubleVector(radii))

```

* メッシュデータの表示、保存など

```python
        # 再度法線計算
        mesh.compute_vertex_normals()
                       
        # 一応データ保存
        o3d.io.write_triangle_mesh("output.ply", mesh)
        
        # メッシュデータの表示
        o3d.visualization.draw_geometries([mesh])

finally:
    # ストリーミング停止
    pipeline.stop()
```

できたのはこちら。
<img width="496" alt="スクリーンショット 2019-11-06 10.51.10.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/491627/4ec8d489-d89c-ab8d-9ebb-e18ac3500ccf.png">

## 終わりに

Open3DとRealSenseSDKの探索をかねて、3Dスキャナのようなものを作ってみました。
点群データを各方向から生成し、Mergeした上で本処理を実行すれば、もう少し3Dデータらしいものを生成できると思います。
それでは。