射影変換により画像から距離を推定する

## 写真から距離を推定したい
ここにペットボトル４本が映った写真があります。
これらペットボトル同士の距離はそれぞれ何cmでしょうか？
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2626665/709159d1-12d1-737a-2da4-21282cc903eb.jpeg" alt="01.PNG" width=60%>

>写真画像上でペットボトル同士の距離を測ることは出来ますが、実際の距離は奥行も加味しないといけないので、この写真だけから距離を推定するのは難しそうです。

では、こちらの写真だとどうでしょうか？
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2626665/a5a4fad2-aefb-24a9-df04-cf0feba3e657.jpeg" alt="01.PNG" width=60%>
こちらは先ほどと同じものを真上から見下ろすように撮った写真です。
これなら奥行きは気にすることなく実際の距離が推定できそうです。

ということは、
「**通常撮影の画像**」⇒「**上から見下ろすように撮った画像**」
のような変換が出来れば、通常画像からでも距離の推定が出来そうです。
※ 以降では「上から見下ろすように撮った画像」を **鳥瞰画像** と呼ぶことにします。

## 鳥瞰画像変換に必要な技術
鳥瞰画像への変換に用いる技術として **射影変換** という写像手法があります。
射影変換（projective transformation）とは、任意の形の四角形を別の形の四角形へ変形する変換です。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2626665/16a7c0b7-c107-cf25-11de-87b5660a3b30.png" alt="01.PNG" width=60%>

また射影変換は、**透視変換**（perspective transformation）や**ホモグラフィ変換**（homography transformation）と呼ばれたりもします。**台形補正**などもこの手法を使って行います。
具体的には、以下のような **3x3** の**射影変換行列**を用いて座標変換を行います。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2626665/588ab794-b69a-530b-3dc2-3307e2e6c141.png" alt="01.PNG" width=40%>

> この 3x3 の上部2x3 だけを使うと**アフィン変換**になります。
よってアフィン変換は射影変換のサブセットという見方をしてもよさそうです。

この行列は、**四角形頂点の変形前後の座標** から求めることが可能です。
今回の写真であれば、畳の四角形を使うと分かり易そうです。
すなわち以下のようにすれば、**ペットボトル実座標**を求めることが出来そうです。
1. 畳四隅の写真上の座標と実座標を取得する
1. 上記座標から射影変換行列を求める
1. 射影変換行列で写真画像を鳥瞰画像に変換する
1. 鳥瞰画像上のペットボトル座標を実座標とみなす
> 各ペットボトルの座標さえ分かってしまえば、ペットボトル同士の距離は「２点間の距離の公式」で簡単に求められます。

## 実際にやってみよう！
では実験に先立ち、必要となる座標情報をあらかじめ取得しておきましょう。
写真上の座標を確認した結果は以下のようになりました（単位：pixel）。

- 畳四隅の座標（緑色）：射影変換行列を求める際に使用 
- ペットボトル座標（赤色）：この座標をプログラムで変換します

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2626665/ec51ba40-2493-abb8-c9c1-7e92807946ce.png" alt="01.PNG" width=70%>

続いて実座標情報の取得です。
これはもう……、頑張って巻き尺で測るしかありません（笑）
測った結果を図にすると以下のようになりました（単位：cm）。

- 畳四隅の座標（緑色）：射影変換行列を求める際に使用
- ペットボトル座標（赤色）：プログラム実行結果の答え合わせ用

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2626665/4fb37a41-40b9-91ac-811a-6b4d594c3550.png" width=90%>

## 射影変換行列を求める
ここからはPythonを使っていきます。
先ずは射影変換行列を求めたいと思います。

> 射影変換行列は**OpneCV**の**getPerspectiveTransform**関数を使って求めます。
下記では変数**M** が射影変換行列となります。その中身も確認しておきます。

```python:sample.ipynb
import numpy as np
import matplotlib.pyplot as plt
import cv2

# 基準とする畳四隅の写真上の座標（単位px）
pts1 = np.array([(29, 950), (1047, 448), (1558, 553), (782, 1422)], dtype=np.float32)
# 基準とする畳四隅の実際の座標（単位cm）
pts2 = np.array([(30,30), (208,30), (208,120), (30,120)], dtype=np.float32)

# 射影行列の取得
M = cv2.getPerspectiveTransform(pts1, pts2) 
np.set_printoptions(precision=5, suppress=True)
print (M)
'''
[[    0.53757    -0.55406   831.92386]
 [    0.81283     1.95866 -1563.14951]
 [   -0.00006     0.01022     1.     ]] 
'''
```
## 鳥瞰画像の生成
続いて、元の写真画像を読み込んで鳥瞰画像を生成してみます。
> 画像の射影変換は **OpneCV**の**warpPerspective**関数で行っています。

```python:sample.ipynb
# 元画像  (W x H) = (2016 x 1512)
img1 = cv2.imread('MyPicture.jpg', cv2.IMREAD_COLOR)
img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2RGB)

# 元画像を射影変換し鳥瞰画像へ
w2, h2 = pts2.max(axis=0).astype(int) + 50 # 鳥瞰画像サイズを拡張（見た目の調整） 
img2 = cv2.warpPerspective(img1, M, (w2,h2) )

# 結果表示 
fig = plt.figure(figsize=(8,8))
fig.add_subplot(1,2,1).imshow(img1)
fig.add_subplot(1,2,2).imshow(img2)
plt.show()
```
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2626665/3a53b962-ecf7-830c-3770-b776e2c5c8de.png" width=120%>

おっ！　鳥瞰画像らしきものが生成されましたね。
> 上記結果は、左が元画像、右が生成した鳥瞰画像となっています。

鳥瞰画像でペットボトルがニョキニョキ成長しているのはご愛敬～（笑）
>………ではなく、真面目に解説すると、この変換は「写真上の全てのものが畳と同一平面上にある」という前提の画像変換となるため、このように畳と同一平面上にないペットボトル上部などは伸長される結果となるのです。。。

## ペットボトル実座標の推定
ではいよいよ、ペットボトルの実座標を推定していきます。
**実座標＝鳥瞰画像上での座標**となっていますので、鳥瞰画像上でのペットボトル座標を求めます。
具体的には、元画像上でのペットボトル座標を**行列M**で変換します。

```python:sample.ipynb
def transform_pt(pt, M):
    '''座標ptを変換行列Mで変換'''
    if isinstance(pt, (list, tuple)):
        pt = np.array(pt, dtype=np.float32)
    assert pt.ndim == 1 and pt.shape[0] == 2 # 1次元のx,y2要素を前提
    pt = np.append(pt, 1.0)
    pt = np.dot(M, pt) # 射影行列で座標変換
    pt = pt / pt[2] # 第3要素が１となるよう按分
    pt = pt[:2] # x,y要素
    return tuple(pt.astype(int).tolist()) # 後でdrawMarkerで使うのでtupleにしておく

# 元画像におけるペットボトル座標
pb_pts = [(955, 903), (1040, 556), (1303, 477), (1580, 759),]

# 鳥瞰画像におけるペットボトル座標に変換
for src_pt in pb_pts:
    dst_pt = transform_pt(src_pt, M)
    print(f'{src_pt} ===> {dst_pt}')
'''
元画像座標 ===> 鳥瞰画像座標
(955, 903) ===> (83, 96)
(1040, 556) ===> (163, 56)
(1303, 477) ===> (218, 74)
(1580, 759) ===> (145, 139)
'''
```
ここで得られた鳥瞰画像座標と、答え合わせ用に最初に測っておいた実座標を比べると.....
|No| 鳥瞰画像上の座標 | 答え合わせ用の実座標 |
|:-:|:-:|:-:|
|1| (83, 96)  |  (84,95) |
|2| (163, 56)  |  (165,55) |
|3| (218, 74)   |  (219,74) |
|4| (145, 139)  |  (146,139) |

ほぼ一致の結果になりました！

## ビジュアルでも確認

最後にビジュアル確認もしておきましょう。
元画像のペットボトルや畳四隅の座標を鳥瞰座標に変換し、鳥瞰画像上にマーキングしてみます。

```python:sample.ipynb
img3 = img2.copy()
# 元画像での畳四隅座標を射影変換し鳥瞰画像にマーキング
for src_pt in pts1:
    dst_pt = transform_pt(src_pt, M)
    cv2.drawMarker(img3, dst_pt, (0, 255, 0))
    
# 元画像でのペットボトル座標を射影変換し鳥瞰画像にマーキング
for src_pt in pb_pts:
    dst_pt = transform_pt(src_pt, M)
    cv2.drawMarker(img3, dst_pt, (255, 0, 0), cv2.MARKER_TILTED_CROSS)

plt.figure(figsize=(8,8))
plt.imshow(img3)
plt.show()
```

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2626665/8a5511f4-fd18-a26a-abae-2a8488e74301.png" width=80%>

鳥瞰画像の期待した位置にマーキング出来ていることは一目瞭然ですね！

測りたいものと同一平面上の四角形（仮想でもよい）の実座標が判っていれば、その平面上にある物体を真上から鳥瞰した画像を生成したり、鳥瞰画像上での座標を求めたりすることが出来ます。
物体検出技術との組み合わせにより、人やモノの距離推定や動線解析などに応用が効きそうです。

以上です。
ここまでお読みいただき、ありがとうございました。

---
::: note 
動作確認環境：
 ・ *python 3.10.11 + opencv 4.7.0 + matplotlib 3.7.1*