pyrealsense2で距離カメラのデータを扱う

<br>
株式会社 日立情報通信エンジニアリング の追立です。
「IoT」が世間一般に浸透してしばらく経ちます。現場の情報収集に必要なセンサ類も、振動、電圧など色々なものが増えました。
距離カメラも様々な機種が安価に販売されるようになっており、距離＋画像のデータが利活用されるようになってきました。

本記事ではPythonから距離カメラを扱うことができるOSSという事でpyrealsense2でいろいろ試行した結果をご紹介します。
ノイズ処理や距離推定は結構使うかな？と思うのですがあまり情報を見つけられなかったのでこの記事が役立てば幸いです。

<br>

# pyrealsense2とは？
pyrealsense2はIntel® RealSense™  SDK 2.0のPythonラッパーです。
Intel®社から市販されている距離カメラIntel® RealSense™シリーズを使用するためのOSSです。
License：Apache License, Version 2.0

※本AdventCalendarの趣旨はOSSであるため、特に必要が無い限りカメラの製品仕様等については説明を省略しています。
　製品情報については公式HPをご参照ください。

# 導入

pipコマンドで導入することができます。

```言語:install
pip3 install pyrealsense2
```

インストールしたモジュールがimport可能なことを確認します。

```python:
import pyrealsense2
```

今回インストールされたバージョンは2.39.0.2342でした。


#距離データとRGB画像を保存する
早速PCにカメラを接続し、距離カメラとRGBカメラの情報を画像として保存します。
なお、今回検証で使用するカメラである「Intel® RealSense™ D435i」はステレオ方式のカメラであり、RGB映像用のカメラとは別に距離データ取得用のカメラが備わっています。よってそれぞれのカメラに対してconfigを設定していきます。

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

# カメラの設定
conf = rs.config()
# RGB
conf.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
# 距離
conf.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)

# stream開始
pipe = rs.pipeline()
profile = pipe.start(conf)

cnt = 0

try:
    while True:
        frames = pipe.wait_for_frames()
        
        # frameデータを取得
        color_frame = frames.get_color_frame()
        depth_frame = frames.get_depth_frame()

        # 画像データに変換
        color_image = np.asanyarray(color_frame.get_data())
        # 距離情報をカラースケール画像に変換する
        depth_color_frame = rs.colorizer().colorize(depth_frame)
        depth_image = np.asanyarray(depth_color_frame.get_data())

        #お好みの画像保存処理


```

ここで、変換したデータ(color_image,depth_image)を画像データとして保存することができます。

#距離情報を取得する
取得したデータに対してピクセルを指定することで、当該ピクセルの距離情報(m)を取得することができます。

```python:
# 距離情報の取得
depth_data = depth_frame.get_distance(x,y)
```

#距離データとRGB映像を動画として保存する
カメラで撮影したデータを動画(.bag)として保存します。
enable_record_to_file()モジュールを先程の画像保存処理に下記のように追加することで保存することができます。

```python:
# 省略 
# カメラの設定
conf = rs.config()
conf.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
conf.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
conf.enable_record_to_file('export_filename.bag')  #ADD

# stream開始
pipe = rs.pipeline()
profile = pipe.start(conf)
# 省略
```

#後処理をする
カメラで撮影したデータや保存済の.bagデータに対して後処理をかけます。
後処理をすることによってノイズを減らすことができるようです。
Pythonラッパーにもいくつかのフィルタが用意されているようですが、今回は以下の三つを試してみました。

+ Decimation Filter : 複雑さを軽減する効果
+ Spatial Filter : 平滑化する効果
+ Hole Filling Filter : 欠損データを補完する効果

```python:後処理(Filter)
# decimarion_filterのパラメータ
decimate = rs.decimation_filter()
decimate.set_option(rs.option.filter_magnitude, 1)
# spatial_filterのパラメータ
spatial = rs.spatial_filter()
spatial.set_option(rs.option.filter_magnitude, 1)
spatial.set_option(rs.option.filter_smooth_alpha, 0.25)
spatial.set_option(rs.option.filter_smooth_delta, 50)
# hole_filling_filterのパラメータ
hole_filling = rs.hole_filling_filter()
# disparity
depth_to_disparity = rs.disparity_transform(True)
disparity_to_depth = rs.disparity_transform(False)

# 省略(フレーム取得処理)

# filterをかける
filter_frame = decimate.process(depth_frame)
filter_frame = depth_to_disparity.process(filter_frame)
filter_frame = spatial.process(filter_frame)
filter_frame = disparity_to_depth.process(filter_frame)
filter_frame = hole_filling.process(filter_frame)
result_frame = filter_frame.as_depth_frame()
```

フィルタをかける前後の結果を比較してみます。
<br>
フィルタをかける前：
![before.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/893284/a66d51e4-3740-86f4-9755-a12ba2e5e36b.png)
<br>
フィルタをかけた後：
![affter.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/893284/58d0f89e-3fa4-104e-fb43-a18b4b06fc9f.png)

<br>


#(応用編)撮った物体のサイズを推定をしてみる
pyrealsense2には画像上のピクセル座標と深度情報を与えるとカメラからの相対的な3次元座標情報を返してくれるモジュールが有ります。
これを使ってカメラに映っている物体のサイズを推定してみます。

今回は↓の画像にある15cmの定規を対象にしてみます。
![color.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/893284/c3507ddb-a437-3c0d-a01a-0e73c3addac9.png)

製品を見ると分かりますがRGB映像用のカメラと距離データ取得用の2つのIRカメラはついてる場所が異なります。
よって取得できるデータもRGBと距離データの間で微妙にずれています。まずはこれを補正します。

```python:補正
# alignモジュールの定義
align_to = rs.stream.color
align = rs.align(align_to)

#     (中間処理は省略)

# frame処理で合わせる
frames = pipe.wait_for_frames()
aligned_frames = align.process(frames)
depth_frame =  aligned_frames.get_depth_frame()
color_frame =  aligned_frames.get_color_frame()
```

次に距離データに後処理(フィルタ)をかけます。
今回はサイズを測ろうとする座標の深度データが必要なので、欠損は補間してやる必要が有ります。
処理自体は前章でご紹介したような内容でフィルタをかけていきます。

処理前：
![depth.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/893284/7d6c32af-abfe-e5b7-60b8-148b666d618a.png)

処理後：
![filter.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/893284/62847c3b-cb9b-289e-aaf6-c80fa1d31f3d.png)

いよいよ画像の座標から3次元座標を求めていきます。
rs2_deproject_pixel_to_point()を使いますが、これに必要なのが画像上の座標情報(x,y)と深度情報とカメラモジュールの内部パラメータです。
今回は画像をRGBカメラ側に補正していますのでRGBカメラモジュールの内部パラメータを取得します。

```python:内部パラメータ取得
color_intr = rs.video_stream_profile(profile.get_stream(rs.stream.color)).get_intrinsics()
```

次に取得する座標情報を決めます。
今回は15cm定規の端から端までの長さを推定してみます。開始点を「イ」、終了点を「ロ」として画像上の座標情報を取得します。

+ イ : (685, 642)
+ ロ : (685, 290)

![color_point.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/893284/b5b1ae24-342e-53dc-c6a5-026fb5e130cd.png)

これらの情報を元に長さを推定してみます。

```python:長さの推定
#イの3次元座標推定
I_d = result_frame.get_distance(685,642)
point_I = rs.rs2_deproject_pixel_to_point(color_intr , [685,642], I_d)
#ロの3次元座標推定
R_d = result_frame.get_distance(685,290)
point_R = rs.rs2_deproject_pixel_to_point(color_intr , [685,290], R_d)

#推定距離を算出
est_range = math.sqrt((Point_I[0]-Point_R[0])*(Point_I[0]-Point_R[0]) + (Point_I[1]-Point_R[1])*(Point_I[1]-Point_R[1]) +(Point_I[2]-Point_R[2])*(Point_I[2]-Point_R[2]))
print(est_range)
#-------------
#結果(0.1471)[m]
```
思ってたより実際の値に近い値が出ました。


#あとがき
普段から業務で色々なデータをPythonで分析している関係上、Pythonラッパーとして提供されているので他の処理との連携がやりやすく感じました。
物体検出と組み合わせることで、カメラ1台から物体の位置などの情報を取得することもできますし、その他のセンサデータとの連携も敷居が低くなっているのではないかと思います。
データ利活用では単独のデータのみを扱うという場面は少ないと思いますので、こういったラッパーが提供されているとそれだけでとっつきやすく感じました。

<br>
# 参考文献
本記事の執筆にあたって以下を参考にさせて頂きました。
公式ドキュメント：https://dev.intelrealsense.com/docs

# その他
Intel®、Intel® RealSense™は、アメリカ合衆国および／またはその他の国における Intel Corporation の商標です。