Azure Kinectとプロジェクター投影面の位置を合わせる

#はじめに
少し前に「複数台のAzureKinectを用いてプロジェクター投影面と位置合わせを行いたい」という話がありまして、TouchDesignerでKinectの位置合わせを試したので簡単に共有したいと思います。

#やったこと

<b>「現実空間に投影されているChArUcoマーカーのコーナーの位置」</b>と<b>「3D空間に配置されているChArUcoマーカーのコーナーの位置」</b>を用意して、これら2つの点群セットに対してレジストレーションによる変換行列を推定しました。推定した変換行列をKinectのポイントクラウドに掛け合わせることで以下のように位置を合わせます。
![TouchDesigner 2020.23680_ C__Users_1to10drive_Desktop_k4a-charuco_main.toe_ 2020-12-22 17-41-24_5.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/216025/860da9fd-6976-ea36-540e-13b3f37a0dea.gif)

同様に2台目のKinectも位置を合わせる
![TouchDesigner 2020.23680_ C__Users_1to10drive_Desktop_k4a-charuco_main.toe_ 2020-12-22 17-41-24_11.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/216025/5c6d5b0b-332f-fa91-79f8-b28a7b378ed2.gif)

2台のグレーのカメラは位置合わせを行ったAzure Kinectで、緑色のカメラはプロジェクターの投影面のみを切り取っています。このままでは実際にモノを認識することはできませんが、点群の背景差分を行った上で投影面で切り取った映像に対して領域分割や矩形検出等を加えることで映像のどの位置にモノが映っているかわかります。
![TouchDesigner 2020.23680_ C__Users_1to10drive_Desktop_k4a-charuco_main.toe_ 2020-12-22 17-41-24_1.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/216025/35d9bd0e-5376-d70a-7caa-b8bf92b0ee49.gif)

複数台のKinectをそれぞれ位置合わせするだけでも少し手間な作業ですが、プロジェクターの投影面にマーカーを用いることで映像への位置合わせも同時に解決できました。

#手順
① プロジェクターでChArUcoマーカーを平面に投影する
② Kinectで撮影したChArUcoマーカーのコーナーの位置（A）を取得する
③ 3D空間上にChArUcoマーカーを配置してコーナーの位置（B）を取得する
④ （A）と（B）から変換行列を推定する


##① プロジェクターでChArUcoマーカーを平面に投影する

###ChArUcoマーカーの作成

ChArUcoマーカーは今後も使えそうなのでtoxにしました。
(※ChArUcoを使用するためにはTouchDesignerのパッケージにopencv-contribが必要です)

>![output1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/216025/8f38616e-1827-56bc-e20c-0a57112c2182.png)
>Board　：ChArUcoマーカーの設定を入力して指定のフォルダにChArUcoマーカー画像を作成する
>Window：プロジェクターで投影するための機能

中身はDATでChArUcoマーカーを作成するコードを書いてます。
![output2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/216025/4458f285-1dee-e356-6f5b-bf7f52a7ea95.png)

###ChArUcoマーカーの投影
これをプロジェクターで投影します。プロジェクターのアスペクトが16:9なのでマーカーも16x9で作成してます。
![output.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/216025/641b7669-00ef-758c-84ac-9462431ab931.png)



##② Kinectで撮影したChArUcoマーカーのコーナーの位置を取得する

Kinectはこんな感じで設置しました。
![output4.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/216025/1c489ff2-469d-80d8-166c-6dc11cea771e.png)

TouchDesigner上でポイントクラウドを可視化するとこんな感じ。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/216025/6ba031d1-fd29-188f-a5fb-80114ac5368d.png)



###Kinectのカラー画像でChArUcoマーカーのコーナーの座標(x, y)を取得する
Kinectのカラー画像からChArUcoマーカーのコーナーを検出します。
>![無題1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/216025/fb727fe8-0ace-28a8-37ff-1e9f5b7adbc8.png)
・soruce
　　カラー画像をグレースケール化した入力画像
・get_2d_corners
　　ChArUcoマーカーのコーナーの座標検出部分
・draw_image
　　ChArUcoマーカーのコーナーを描画した検出結果画像（デバッグ用）
・checkervoard_2d_corners
　　検出したコーナーの番号と座標（ピクセルの位置）



###検出したコーナーの座標を元に位置(x, y, z)を取得する
検出したコーナーの座標（ピクセル位置）を元にKinectのデプス画像から深度値を取得します。
>![無題3.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/216025/ecca1e32-dad2-8fa5-ae53-e9d7c9530ca5.png)
・checkerboard_2d_corners
　　検出したコーナーの番号と座標
・pointcloud
　　Kinectのポイントクラウド
・get_3d_corners
　　ChArUcoマーカーのコーナーの位置検出部分
・checkervoard_3d_corners
　　検出したコーナーの番号と位置

検出したコーナーの位置(checkerboard_3d_corners)を可視化するとこうなります。
（黄緑の点が検出したコーナー）
![無題4.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/216025/61cddf7a-799a-0f39-be5d-8292cb9ad428.png)



##③ 3D空間上にChArUcoマーカーを配置してコーナーの位置を取得する
charuco.toxからChArUcoマーカーの情報を取得してcheckerboard, grid, checkerboard_cornersを配置した図です。
赤い点が各コーナーの位置を示しており、checkerboard_corners_worldPositionに位置を格納しています。
![無題5.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/216025/e3bc1957-e451-6821-2558-d60870675d7e.png)

レジストレーションの際にはKinect側で検出したコーナーのみ使用するため、使わないコーナーを取り除きます。
（黄緑がKinectで検出したコーナー、赤が3D空間上に配置したコーナー）
![無題7.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/216025/8f33a4ec-45aa-e617-2dc9-dce2bbf72a3f.png)

##④ 変換行列を推定する
２つの点群セットを入力として変換行列を推定します。
>![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/216025/0bbaf27c-0558-0f24-cab2-cfb70f7d7e76.png)
・in1
　　<b>Kinectで撮影したChArUcoマーカーのコーナーの位置</b>
・in2
　　<b>3D空間上にChArUcoマーカーを配置してコーナーの位置</b>


caluculate_matrix4x4でOpen3Dのレジストレーションによる変換行列の推定を行っています。
http://www.open3d.org/docs/0.7.0/python_api/open3d.registration.TransformationEstimationPointToPoint.html
（※使用する場合はOpen3Dのパッケージを追加する必要があります）

```python:calculate_matrix4x4
import open3d as o3d
import numpy as np

sourcePoints = np.empty((0,3), float)
targetPoints = np.empty((0,3), float)

for point in op('in1').rows():
    sourcePoints = np.append(sourcePoints, np.array([[float(point[0].val), float(point[1].val), float(point[2].val)]]), axis=0)

for point in op('in2').rows():
    targetPoints = np.append(targetPoints, np.array([[float(point[0].val), float(point[1].val), float(point[2].val)]]), axis=0)

source = o3d.geometry.PointCloud()
target = o3d.geometry.PointCloud()

source.points = o3d.utility.Vector3dVector(sourcePoints.tolist())
target.points = o3d.utility.Vector3dVector(targetPoints.tolist())

transformEstimation = o3d.registration.TransformationEstimationPointToPoint()
transformEstimation.with_scaling = True

corres = o3d.utility.Vector2iVector(np.repeat(np.arange(op('in1').numRows), 2).reshape(op('in1').numRows,2))

m = transformEstimation.compute_transformation(source, target, corres)
matrix = tdu.Matrix(m[0],m[1],m[2],m[3])
matrix.fillTable(op('result'))
```

##おわり
もう少し整理できればよかったんですが時間がきたのでここまで..
また時間があるときに説明だけでも追加しようと思います。

（ここを詳しくなどリクエストがあればご連絡ください）