Python+OpenCVで、カメラ画像から机上の物体位置（実座標系）を計測してみる

# はじめに

次のように**基準マーカー付きの平面（机）に置いた任意物体の位置を、実座標系で求めること**を目標にします。
実装は Python で行ない、OpenCV の **ArUco** というモジュールを利用します。
![概要.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/4108f383-60b4-e685-5619-3b4238731dfe.png)

- [OpenCV/ArUcoライブラリで物体位置計測](https://inody1991.tumblr.com/post/172449790255/opencvaruco%E3%83%A9%E3%82%A4%E3%83%95-%E3%83%A9%E3%83%AA%E3%81%A6-%E7%89%A9%E4%BD%93%E4%BD%8D%E7%BD%AE%E8%A8%88%E6%B8%AC) @ inoblog を参考にさせていただきました。
 
## 実行環境

GoogleColab.（Python 3.6.9）を利用します。使用したモジュールのバージョンは次のようになります。

```py:各種モジュールのバージョン
%pip list | grep -e opencv -e numpy
numpy                    1.17.5     
opencv-contrib-python    4.1.2.30   
opencv-python            4.1.2.30   
```

# ArUcoによるマーカーの作成

はじめに、処理に必要なモジュールを読み込みます。

GoogleColab. 環境では、`cv2.imshow(...)` が利用できないのですが、次のようにすることで、 `cv2_imshow(...)` で画像（np.ndarray）を実行結果セルに出力することができます。

```py:モジュールのインポート
import cv2
import numpy as np
from google.colab.patches import cv2_imshow
```

ArUco には、**あらかじめ定義されてるマーカー群が用意されています**。今回は、それを利用します。`aruco.getPredefinedDictionary(...)` により、事前定義されたマーカーが格納されている辞書を取得します。引数 `aruco.DICT_4X4_50` により、正方形の内部に $4\times 4$ の塗りつぶしパターンを持ったマーカーが最大 $50$ 個まで利用可能な辞書を選択しています。

```py:事前定義されているマーカー（0～3番）を画像ファイルとして保存
aruco = cv2.aruco
p_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
marker =  [0] * 4 # 初期化
for i in range(len(marker)):
  marker[i] = aruco.drawMarker(p_dict, i, 75) # 75x75 px
  cv2.imwrite(f'marker{i}.png', marker[i])
```

上記を実行すると「marker0.png」から「marker3.png」までの $4$ つのファイル（$75\times 75$ピクセルサイズ）が出力されます。

GoogleColab.環境であれば、作成されたマーカーは `cv2_imshow(...)` で出力セルに表示して確認できます。正方形のなかに $4\times 4$ の塗りつぶしパターンを持ったマーカーが作成されていることが分かります。
![cv2_show_marker.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/7eedd612-f2b9-3def-8963-644fd708019a.png)
もし、`aruco.getPredefinedDictionary(...)` の引数に `DICT_5X5_100` を与えれば、$5\times 5$ のパターンで最大 $100$ 個までのマーカーが定義された辞書を取得できます。
![cv2_show_marker_5x5.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/86e965ed-c3e5-a8e6-91f4-e2240faa8c75.png)

# ArUcoによるマーカーの検出

「marker0.png」つまり `aruco.DICT_4X4_50` の 0番目のマーカーを出力したものを紙に印刷します。そして、それをカメラで撮影したものを「m0-photo.jpg」とします。これを対象にマーカー検出し、その検出結果をオーバーレイした画像 `img_marked` を出力します。

```py:0番マーカーを含んだ写真から検出
img = cv2.imread('m0-photo.jpg')
corners, ids, rejectedImgPoints = aruco.detectMarkers(img, p_dict) # 検出
img_marked = aruco.drawDetectedMarkers(img.copy(), corners, ids)   # 検出結果をオーバーレイ
cv2_imshow(img_marked) # 表示
```

![m0_photo_marked.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/bdeced08-77a8-758e-a268-a5f6b15f5401.png)

マーカー全体が**<font color='green'>緑色の四角形**</font>、マーカー左上が**<font color='red'>赤色の四角形**</font>、マーカーID（番号）が**<font color='blue'>青色の文字</font>**で元画像に重ねて描画されています。これより、マーカー検出が適切にできていることが分かります。

`aruco.detectMarkers(...)` の**戻値**について詳しく見ていきます。

`corners` には、画像から検出された各マーカーの**コーナー（四隅）の画像座標**が、np.ndarray のリストで格納されます。

```py:aruco.detectMarkers(...)の戻値corners
# ■ 戻値 corners
# print(type(corners)) # -> <class 'list'>
# print(len(corners))  # -> 1
# print(type(corners[0]))  # -> <class 'numpy.ndarray'>
# print(corners[0].shape)  # -> (1, 4, 2)
print(corners)
```

```:実行結果
[array([[[265., 258.],
        [348., 231.],
        [383., 308.],
        [297., 339.]]], dtype=float32)]
```

コーナーの座標は、マーカーの左上、右上、右下、左下の順番で反時計周りで格納されています。つまり、次のようになります。
![m0-photo1_.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/1390db10-c00f-1365-9e29-0b0316289b26.png)

`ids` には、画像から検出された**マーカーのID**が numpy.ndarray 形式で格納されます。

```py:aruco.detectMarkers(...)の戻値ids
# ■ 戻値 ids
# print(type(ids)) # -> <class 'numpy.ndarray'>
# print(ids.shape) # -> (1, 1)
print(ids)
```

実行結果は `[[0]]` のようになります（`[0]` ではない点に注意してください）。

`rejectedImgPoints` には、画像内に検出された正方形のうち、その内部に適切なパターンを含まない正方形の座標が格納されます（[原文](https://docs.opencv.org/4.1.2/d9/d6a/group__aruco.html#gab9159aa69250d8d3642593e508cb6baa) contains the imgPoints of those squares whose inner code has not a correct codification. Useful for debugging purposes.）。

これについては、具体例を見てもらったほうが分かりやすいと思います。`rejectedImgPoints` には、図内の矢印で指しているような正方形の座標が格納されます。
![rejectedImgPoints.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/6eb1ac13-c5e2-cbbf-d8bf-c523795ed05b.png)

# ArUcoによるマーカーの検出（マーカーが複数ある場合）

次のように画像内に複数のマーカーが存在する場合（同じマーカーが2つ以上存在する場合を含む）の結果について確認しておきます。
![ms-photo.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/87dea3f3-639e-ff36-6017-38a821563e3d.jpeg)
検出のためのコードは先と同じです。検出結果をオーバーレイすると次のようになります。同じマーカーが2つ以上存在しても問題ありません。
![ms_photo_marked.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/27808211-d1e7-130a-387d-ec4716d19d76.png)

`aruco.detectMarkers(...)` の戻値 `ids` と `corners` は次のようになります。ID順にはソートされていない点に注意します。

```:print(ids)の結果
[[1]
 [1]
 [0]
 [2]]
```
```:print(corners)の結果
[array([[[443., 288.],
        [520., 270.],
        [542., 345.],
        [464., 363.]]], dtype=float32), array([[[237., 272.],
        [313., 255.],
        [331., 328.],
        [254., 346.]]], dtype=float32), array([[[ 64., 235.],
        [140., 218.],
        [154., 290.],
        [ 75., 307.]]], dtype=float32), array([[[333., 113.],
        [404.,  98.],
        [424., 163.],
        [351., 180.]]], dtype=float32)]
```

# 実座標系における位置計測

最初に作成した4つのマーカー「marker0.png」～「marker3.png」を、次のように時計まわりに配置した紙を用意します。ここでは、マーカー間の距離を $150\mathrm{mm}$ としました（この用紙作成は、厳密に行なわないと精度が悪くなります）。
![table.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/8a762359-fe5f-51e6-5ce5-9ca5ddb7a671.png)

この紙の上に、位置検出したい対象物「イヌ」を配置して、**上方の適当な位置**から撮影します（このときの撮影位置や角度は厳密である必要はありません）。目的は、この「イヌ」の位置を**マーカーを基準とした実座標系**で求めることです。
![inu.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/7d8b4c7a-961a-d996-4de8-afee52b20264.png)

## 真上から見た画像に変換

上方の適当な位置・角度から撮影したカメラ画像を、**真上から見た画像**に変換していきます。ここでは、変換後の画像が $500\times 500\, \mathrm{px}$ になるようにしました。

```python:真上から見た画像に変換
aruco = cv2.aruco
p_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
img = cv2.imread('inu.jpg')
corners, ids, rejectedImgPoints = aruco.detectMarkers(img, p_dict) # 検出

# 時計回りで左上から順にマーカーの「中心座標」を m に格納
m = np.empty((4,2))
for i,c in zip(ids.ravel(), corners):
  m[i] = c[0].mean(axis=0)

width, height = (500,500) # 変形後画像サイズ

marker_coordinates = np.float32(m)
true_coordinates   = np.float32([[0,0],[width,0],[width,height],[0,height]])
trans_mat = cv2.getPerspectiveTransform(marker_coordinates,true_coordinates)
img_trans = cv2.warpPerspective(img,trans_mat,(width, height))
cv2_imshow(img_trans)
```
実行結果は、次のようになります。各マーカの**中心**が画像の四隅になるように変形されていることが分かります。
![inu_trans.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/1c72f325-ea23-5c3f-e9bd-808494f8244a.png)
もっと斜めの位置から撮影した画像を使っても、次のように真上から見たような画像に変換することができます。
![coin.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/81ef741a-6512-ee62-e7fa-65ef5bae3a85.png)

ここでは分かりやすいように、各マーカーの**中心**が変換後画像の四隅になるようにしました。しかし「イヌ」の位置を計算するためには何かと都合が悪いです（四隅にマーカーの断片が写り込んでいることも・・・）。

そこで、紙上の $150\times 150\mathrm{mm}$ の四角形に接している各マーカーの角が、変換後画像の四隅になるようにプログラムに手を加えます。

```python:真上から見た画像に変換（改良版）
aruco = cv2.aruco
p_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
img = cv2.imread('inu.jpg')
corners, ids, rejectedImgPoints = aruco.detectMarkers(img, p_dict) # 検出

# ここを変更
corners2 = [np.empty((1,4,2))]*4
for i,c in zip(ids.ravel(), corners):
  corners2[i] = c.copy()
m[0] = corners2[0][0][2]
m[1] = corners2[1][0][3]
m[2] = corners2[2][0][0]
m[3] = corners2[3][0][1]

width, height = (500,500) # 変形後画像サイズ
marker_coordinates = np.float32(m)
true_coordinates   = np.float32([[0,0],[width,0],[width,height],[0,height]])
trans_mat = cv2.getPerspectiveTransform(marker_coordinates,true_coordinates)
img_trans = cv2.warpPerspective(img,trans_mat,(width, height))
cv2_imshow(img_trans)
```

実行結果は、次のようになります。
![inu_trans2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/590315c6-34de-112e-55b0-1411f9142ae7.png)
この画像の大きさは $500\times 500\, \mathrm{px}$ で、それに対応する紙上の大きさは $150\times 150\, \mathrm{mm}$ です。よって、画像上の座標が $(160\mathrm{px},\,200\mathrm{px})$ であれば、対応する実座標は $(160\times\frac{150}{500}=48\mathrm{mm} ,\, 200\times\frac{150}{500}=60\mathrm{mm})$ のように求めることがでます。

## 画像からの物体検出

OpenCV で用意されている各種関数を使って、上記画像からイヌの位置を求めていきます。特にポイントとなるのは `cv2.connectedComponentsWithStats(...)` です。

```py:オブジェクト検出と実座標変換後の位置出力
tmp = img_trans.copy()

# (1) グレースケール変換
tmp = cv2.cvtColor(tmp, cv2.COLOR_BGR2GRAY)
#cv2_imshow(tmp)

# (2) ぼかし処理
tmp = cv2.GaussianBlur(tmp, (11, 11), 0)
#cv2_imshow(tmp)

# (3) 二値化処理
th = 130 # 二値化の閾値(要調整）
_,tmp = cv2.threshold(tmp,th,255,cv2.THRESH_BINARY_INV) 
#cv2_imshow(tmp)

# (4) ブロブ（＝塊）検出
n, img_label, data, center = cv2.connectedComponentsWithStats(tmp)

# (5) 検出結果の整理
detected_obj = list() # 検出結果の格納先
tr_x = lambda x : x * 150 / 500 # X軸 画像座標→実座標 
tr_y = lambda y : y * 150 / 500 # Y軸 　〃
img_trans_marked = img_trans.copy()
for i in range(1,n):
  x, y, w, h, size = data[i]
  if size < 300 : # 面積300px未満は無視
    continue
  detected_obj.append( dict( x = tr_x(x),
                              y = tr_y(y),
                              w = tr_x(w),
                              h = tr_y(h),
                              cx = tr_x(center[i][0]),
                              cy = tr_y(center[i][1])))  
  # 確認
  cv2.rectangle(img_trans_marked, (x,y), (x+w,y+h),(0,255,0),2)
  cv2.circle(img_trans_marked, (int(center[i][0]),int(center[i][1])),5,(0,0,255),-1)

# (6) 結果の表示
cv2_imshow(img_trans_marked)
for i, obj in enumerate(detected_obj,1) :
  print(f'■ 検出物体 {i} の中心位置 X={obj["cx"]:>3.0f}mm Y={obj["cy"]:>3.0f}mm ')
```
実行結果は次のようになります。
![inu_trans_marked.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/0118a2bf-0551-c57d-9a53-cb8a34f653ba.png)

```:実行結果
■ 検出物体 1 の中心位置 X=121mm Y= 34mm
```

定規を置いて、上記結果が適切であるかを確認してみます。ちゃんと求まっているようです。
![inu_pos.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/3c54653e-8541-c279-1c1a-23a6cda1cbf3.png)

## 追加実験

もっとたくさんの物体を置いて確認してみます。
![m4-photo2.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/df7d92cc-9bce-0bce-4450-e6f7b9b964d5.jpeg)
二値化の際の「閾値の調整」が必要でしたが、うまくいきました。
検出物体1が「犬（茶色）」、検出物体2が「犬（灰色）」、検出物体3が「うさぎ」、検出物体4が「クマ」になります。
![m4-result.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/dde13d32-8827-c84c-b38e-61dc5cb799d9.png)

# 次のステップ

- 機械学習（画像分類）と組み合わせてみる（画像内の各物体を「イヌ」「ウサギ」「クマ」に分類）。
- ロボットアームと組み合わせたデモを作成してみる。

![robot_arm.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/17318f24-0ad3-a465-4025-708ae81b81a4.jpeg)


### 関連エントリ

- [TensorFlow2 + Keras による画像分類に挑戦](https://qiita.com/code0327/items/7d3c7bd3327ff049243a)
- [Googleの事前学習済みモデルを利用した一般物体検出（TensorFlow Hub経由）](https://qiita.com/code0327/items/3b23fd5002b373dc8ae8)