Raspberry PiでArUcoを使ってみる

# Raspberry PiでArUcoを使ってみる

少し古い技術ですが、ロボット系の自己位置推定で見かけることがある
ARマーカーを試してみたくなり
入手したRaspberryPiでArUcoを認識させてみました。

## やったこと
実行結果の見ためはこんな感じ。(https://www.youtube.com/watch?v=aepFM_JsxbU)
マーカーの輪郭とID,xyz軸を表示しています。　ピッチロールヨーも取得できるので
様々な用途に使えそう。
![2020-05-03-135817_1920x1080_scrot.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/483299/27ce4469-dd89-e390-e59a-eb16f4e4fe1b.png)



環境
・RasPi4　(RasPi3でも動くはずです)
・USBカメラ(Logicool)　→　Raspi camでも可能。

## RasPiのセットアップ

まずは”からあげ”さんの下記の記事通りセットアップ。
認識にはOpenCVが必要です。
非常にスムーズにできました。いつも感謝です。
・[Raspberry Pi 4のディープラーニングで画像認識する環境をゼロから1時間で構築する方法](https://karaage.hatenadiary.jp/entry/rpi4-dl-setup)

```py:OpenCVセットアップ
git clone https://github.com/karaage0703/raspberry-pi-setup
cd raspberry-pi-setup && ./setup-opencv-raspbian-buster.sh
```

追加でパッケージをインストール
記事通り、いったん全部入れました。
* [Raspberry Pi 4の強化学習用のセットアップ](https://note.com/npaka/n/n034c8ee6e5cc)
※追記　pipだけでもよい。
* [Raspberry Pi 4 に OpenCV を楽に入れる](https://qiita.com/naoppy/items/8a2178383859178e6d7e)
```py:install
pip3 install opencv-contrib-python==4.1.0.25
```

<details><summary>旧手順</summary>

```py:install
sudo apt-get install libjpeg-dev libtiff5-dev libjasper-dev libpng-dev
sudo apt-get install libavcodec-dev libavformat-dev libswscale-dev libv4l-dev
sudo apt-get install libxvidcore-dev libx264-dev
sudo apt-get install libatlas-base-dev gfortran
sudo apt-get install libhdf5-dev libhdf5-serial-dev libhdf5-103
sudo apt-get install libqtgui4 libqtwebkit4 libqt4-test python3-pyqt5
pip3 install opencv-contrib-python==4.1.0.25
```

```py:check
python3 check_cv.py
```

```py:check cv2
import cv2
print(cv2.__version__)

camera = cv2.VideoCapture(0)
while True:
   _, im_org = camera.read()
   cv2.imshow('check_cv', im_org)
   cv2.waitKey(1)
```
</details>

これで準備完了。

## ARマーカーを認識する手順
マーカーの生成　→生成は下記リンク参照.
・(カメラキャリブレーション)　→　取りあえず動かすなら後回し。
・マーカーの認識　
・[pythonでARマーカーの姿勢推定](https://qiita.com/ReoNagai/items/a8fdee89b1686ec31d10)

## 実装

・コピペで使えるはずです。

```python:ARdetect.py

#!/usr/bin/env python
# -*- coding: utf-8 -*

import numpy as np
import cv2
from cv2 import aruco

def main():
    cap = cv2.VideoCapture(1) #使うカメラによって数値を変更
    # マーカーサイズ
    marker_length = 0.056 # [m]
    # マーカーの辞書選択
    dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

    #camera_matrix = np.load("mtx.npy")
    #distortion_coeff = np.load("dist.npy")
    #カメラキャリブレーションを行った場合、上記を使用。
    camera_matrix = np.array( [[1.42068235e+03,0.00000000e+00,9.49208512e+02],
    [0.00000000e+00,1.37416685e+03,5.39622051e+02],
    [0.00000000e+00,0.00000000e+00,1.00000000e+00]] )
    distortion_coeff = np.array( [1.69926613e-01,-7.40003491e-01,-7.45655262e-03,-1.79442353e-03, 2.46650225e+00] )

    while True:
        ret, img = cap.read()
        corners, ids, rejectedImgPoints = aruco.detectMarkers(img, dictionary)
        aruco.drawDetectedMarkers(img, corners, ids, (0,255,255))

        if len(corners) > 0:
            # マーカーごとに処理
            for i, corner in enumerate(corners):

                rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corner, marker_length, camera_matrix, distortion_coeff)

                # 不要なaxisを除去
                tvec = np.squeeze(tvec)
                rvec = np.squeeze(rvec)
                # 回転ベクトルからrodoriguesへ変換
                rvec_matrix = cv2.Rodrigues(rvec)
                rvec_matrix = rvec_matrix[0] # rodoriguesから抜き出し
                # 並進ベクトルの転置
                transpose_tvec = tvec[np.newaxis, :].T
                # 合成
                proj_matrix = np.hstack((rvec_matrix, transpose_tvec))
                # オイラー角への変換
                euler_angle = cv2.decomposeProjectionMatrix(proj_matrix)[6] # [deg]

                print("ID : " + str(ids[i]))
 
                # 可視化
                draw_pole_length = marker_length/2 # 現実での長さ[m]
                aruco.drawAxis(img, camera_matrix, distortion_coeff, rvec, tvec, draw_pole_length)
        
        cv2.imshow('drawDetectedMarkers', img)
        if cv2.waitKey(10) & 0xFF == ord('q'):
            break
    cv2.destroyAllWindows()

if __name__ == '__main__':
    main()




```

## マーカー　(テスト用)
　生成が面倒な場合にお使いください。

　　　　　　　　　　![1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/483299/9e71c470-4b71-d633-aed1-88e9335a321a.png)

## マーカーの生成

```py:generator
#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import cv2

aruco = cv2.aruco
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

def arGenerator():
    fileName = "ar.png"
    # 0: ID番号，100x100ピクセル
    generator = aruco.drawMarker(dictionary, 0, 100)
    cv2.imwrite(fileName, generator)

    img = cv2.imread(fileName)
    cv2.imshow('ArMarker',img)
    cv2.waitKey(0)
arGenerator()
```

10枚のマーカー作成し、それらを5×5に配置して1枚の画像として出力
```py:generator
import cv2.aruco
import numpy as np

aruco = cv2.aruco
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
SIZE = 150
ImgList1 = []
ImgList2 = []
WiteList = []
def arGenerator():
    img_white = np.ones((SIZE,SIZE, 3),np.uint8)*255
    for i in range(1,6):
        fileName = "ar_" + str(i) + ".png"
        generator = aruco.drawMarker(dictionary, i, SIZE)
        cv2.imwrite(fileName, generator)
        ImgList1.append(cv2.imread(fileName))
        ImgList1.append(img_white)
        WiteList.append(img_white)
        WiteList.append(img_white)
        convImg1 = cv2.hconcat(ImgList1)
        convWhite = cv2.hconcat(WiteList)
    for i in range(6,11):
        fileName = "ar_" + str(i) + ".png"
        generator = aruco.drawMarker(dictionary, i, SIZE)
        cv2.imwrite(fileName, generator)
        ImgList2.append(cv2.imread(fileName))
        ImgList2.append(img_white)
        convImg2 = cv2.hconcat(ImgList2)
    TestList = [convImg1, convWhite, convImg2]
    convImg3 = cv2.vconcat(TestList)
    cv2.imshow('ArMarker1',convImg3)
    cv2.imwrite("Result.jpg", convImg3)
    cv2.waitKey(0)

arGenerator()
```

## 参考サイト
* [Detection of ArUco Markers {#tutorial_aruco_detection}](https://github.com/opencv/opencv_contrib/blob/master/modules/aruco/tutorials/aruco_detection/aruco_detection.markdown#selecting-a-dictionary)
* [Aruco in OpenCV　の解説](https://charmie11.hatenablog.com/entry/2019/04/11/aruco-in-opencv-2/)
* [ArUco マーカーの検出](https://qiita.com/mkisono/items/cfdb9b74e41fae2f59d0)
* [Detection of ArUco Markers](https://docs.opencv.org/3.2.0/d5/dae/tutorial_aruco_detection.html)
* [ArUcoマーカーでカメラ位置姿勢の取得](https://qiita.com/namahoge/items/69b4e2c66f5446dc8798?utm_campaign=popular_items&utm_medium=feed&utm_source=popular_items)
* [ARマーカー作成とARでの画像表示](https://elsammit-beginnerblg.hatenablog.com/entry/2020/10/10/125246)

# マーカーについて　
　※[Aruco in OpenCV　の解説](https://charmie11.hatenablog.com/entry/2019/04/11/aruco-in-opencv-2/)から転記
# dictionary(マーカのセット)は2種類
 1. predefined dictionary: あらかじめ用意されているマーカ 
 2. custom dictionary: 自作するdictionary

## Predefined dictionaryは2種類
 1. Aruco Original: 標準セット
 2. AprilTag: AprilTag
    cv2.aruco.getPredefinedDictionary()を使って取得．
    AprilTagは異なるマーカ間のハミング距離が保証されている一方で，Arucoの標準セットはされていないっぽい*．

### Aruco Original dictionaryはビット数が異なる4種類
 1. DICT_4X4_*: 16 ( = 4x4)ビット
 2. DICT_5X5_*: 25 ( = 5x5)ビット
 3. DICT_6X6_*: 36 ( = 6x6)ビット
 4. DICT_7X7_*: 49 ( = 7x7)ビット

### AprilTagはビット数・最小ハミング距離が異なる4種類
"%dh%d"という表記になっているが，前半はビット数，後半は異なるマーカ間の最小ハミング距離．
最小ハミング距離とマーカの数はdictionary.hppの159−162行に書いてある．
この表を見る限り，36h11を使うべきかなぁ．
()内の数字はマーカの数
1. DICT_APRILTAG_16h5 (30)
2. DICT_APRILTAG_25h9 (35)
3. DICT_APRILTAG_36h10 (2320)
4. DICT_APRILTAG_36h11 (587)


## custom dictionaryは自作マーカ用
 cv2.aruco.custom_dictionary(nMarkers, markerSize, randomSeed)関数を使えば作れる．
 この関数で作られるマーカ(おそらくAruco標準マーカも)は回転の不変性を考慮し，1つのコードに対して4パターン(0,90,180,270度回転)のコードを保持する．
 - nMarkers: マーカの数
 - markerSize: 1次元のビット数 (markerSize x markerSizeのマーカを作成)
 - randomSeed: 乱数のシード

bytesList
生成された2次元コードの情報を保持する．
3次元numpy.array型のオブジェクト．
dictionary.hppの51-60行に書いてあるけど，bytesList[i, j, k]はk番目に回転したi番目のマーカのj番目のバイトを表す．


# その他
※'20 5/12コード内の不要な文字削除
※'22 11/7 追加調査内容を追記
以上、参考になれば。