RealSenseをLiDARのように使い、インタラクティブな壁を作った

インタラクティブな壁を作りました。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">RealSenseをLidarみたいに使って、壁にタッチしたり、絵描いたりできるようになった <a href="https://twitter.com/hashtag/D435?src=hash&amp;ref_src=twsrc%5Etfw">#D435</a> <a href="https://twitter.com/hashtag/RealSense?src=hash&amp;ref_src=twsrc%5Etfw">#RealSense</a> <a href="https://twitter.com/hashtag/%E3%83%81%E3%83%BC%E3%83%97%E3%83%A9%E3%83%9C?src=hash&amp;ref_src=twsrc%5Etfw">#チープラボ</a> <a href="https://t.co/XU04ZPTDMn">pic.twitter.com/XU04ZPTDMn</a></p>&mdash; 藤本賢志(ガチ本)@HoloLens2 Ready (@sotongshi) <a href="https://twitter.com/sotongshi/status/1209720638878838784?ref_src=twsrc%5Etfw">December 25, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

# 実験環境
### ハードウェア
- 壁
- [RealSense D435](https://www.switch-science.com/catalog/3633/)
- [Anker Nebula Capsule](https://www.amazon.co.jp/gp/product/B072Q7CM7S/ref=ppx_yo_dt_b_search_asin_title?ie=UTF8&psc=1)
- PC

### ソフトウェア
- Windows 10
- Anaconda 
- Python 3.6.8
- pyrealsense, opencv-python, numpy
- RealSense SDK

# 環境設定

RealSense D435を床に置きます。設置位置からプロジェクションのスクリーンの底辺までの距離をZ_MINとします。
D435の水平視野角は91.2°なので、黄色い点線から内側が見える感じですね。プロジェクションのスクリーン幅（SC_WIDTH）の最大は、2*Z_MIN*tan(91.2°/2)。

> D435：深度センサ視野角（水平 x 垂直 x 斜め）91.2° x 65.5° x 100.6°（±3°）
> D415：深度センサ視野角（水平 x 垂直 x 斜め）69.4° x 42.5° x 77°（±3°）

![01.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/63863/1b9bb211-d853-eead-f346-3a7ba7ea5f80.png)

プロジェクタを設置します。赤い枠に映像が映るようにしています。
![80803202_1593190520830448_1336100068779884544_n.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/63863/ea9a2b6a-c765-cf94-0296-ba895dfd3b7a.jpeg)

適宜、壁をタッチしたい場所や環境に合わせて、パラメータを調整してください。

# プログラム

RealSenseのDepth画像から真ん中のライン（Y=0）をスキャンします。XとZの値を保持して、それを表示しています。

~~~python
import pyrealsense2 as rs
import numpy as np
import cv2

# D435 Settings
RS_WIDTH = 848
RS_HEIGHT = 480
TARGET_DISTANCE = 10
align = rs.align(rs.stream.color)
pipe = rs.pipeline()
cfg = rs.config()
cfg.enable_stream(rs.stream.depth, RS_WIDTH, RS_HEIGHT,  rs.format.z16, 90)
cfg.enable_stream(rs.stream.color, RS_WIDTH, RS_HEIGHT, rs.format.bgr8, 60)
profile = pipe.start(cfg)
intr = profile.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics()
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
distance_max = TARGET_DISTANCE/depth_scale

# Screen Settings
cv2.namedWindow("screen", cv2.WINDOW_NORMAL)
cv2.setWindowProperty("screen", cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)
screen = cv2.imread("screen.png", -1)
height, width, channels = screen.shape[:3]
SC_WIDTH = 1530 # mm
SC_HEIGHT = 870 # mm
SC_CENTER_X = int(SC_WIDTH/2)
SC_CENTER_Y = int(SC_HEIGHT/2)

SC_IMAGE_WIDTH = 854 # pix
SC_IMAGE_HEIGHT = 480 # pix
SC_IMAGE_CENTER_X = int(SC_IMAGE_WIDTH/2)
SC_IMAGE_CENTER_Y = int(SC_IMAGE_HEIGHT/2)

X_MIN = int(-SC_WIDTH/2)
X_MAX = int(SC_WIDTH/2)
Z_MIN = 1280
Z_MAX = Z_MIN + SC_HEIGHT
OFFSET_X = 20
OFFSET_Z = 110

while True:
    frames = pipe.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 = np.asanyarray(color_frame.get_data())
    depth_image = np.asanyarray(depth_frame.get_data())

    scan_pts = []
    for uy in range(RS_HEIGHT):
        if uy == int(RS_HEIGHT/2) - 7:
            for ux in range(RS_WIDTH):
                z = depth_frame.get_distance(ux, uy)
                x = (float(ux) - intr.ppx) * z / intr.fx
                y = (float(uy) - intr.ppy) * z / intr.fy
                scan_pts.append([x, y, z])

    sc_image = np.zeros((SC_IMAGE_HEIGHT, SC_IMAGE_WIDTH, 3), np.uint8)
    for p in scan_pts:
        y = p[1]*1000
        if abs(y) < 1:
            x = -p[0]*1000 + OFFSET_X
            z =  p[2]*1000 + OFFSET_Z
            if abs(x) < X_MAX:
                if z > Z_MIN and z < Z_MAX:
                    ux = int((x + X_MAX) / SC_WIDTH * SC_IMAGE_WIDTH)
                    uy = SC_IMAGE_HEIGHT - int((z - Z_MIN) / SC_HEIGHT * SC_IMAGE_HEIGHT)
                    if ux >= 0 and ux <= SC_IMAGE_WIDTH and uy >= 0 and uy < SC_IMAGE_HEIGHT:
                        sc_image[uy, ux, 0] = 255 # B
                        sc_image[uy, ux, 1] = 255 # G
                        sc_image[uy, ux, 2] = 255 # R

    cv2.imshow("screen", sc_image)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
~~~

# まとめ

隣同士の点群の距離が短いものをひとまとまりにクラスタリングして、そのクラスタの大きさから手くらいの大きさだけに反応するようにすれば、壁にタッチしたらなんかするとか、文字書いたりもできますね。