Raspberry Pi OpenCVで顔検出しサーボでトラッキング

#はじめに
Aliexpressからカメラのパン/チルト雲台が届いたので、OpenCVで顔検出しサーボで顔をトラッキングするシステムを作成してみることにした。

![DSD_2002.JPG](https://qiita-image-store.s3.amazonaws.com/0/211703/33cb5613-3bfa-a2a5-5a2a-d8a02ded1df2.jpeg)

#用意するもの
##Raspberry Pi 1 Model B
2014年の古いラズパイです。能力がぜんぜん足りず8秒ほど遅延してしまう結果となりました。最新なら問題なくリアルタイム処理できそうです。
##サーボ駆動カメラ雲台
Aliexpressで購入。ST90サーボ2個つきでUS$5.89でした。
![1set-Nylon-FPV-Pan-tilt-Camera-Mount-2pcs-SG90-9g-Servo-Retail-Promotion-Dropship-Free-Shipping.jpg](https://qiita-image-store.s3.amazonaws.com/0/211703/1ddb2e4e-ea26-b3e8-5535-8237a26cc44b.jpeg)
##16ch サーボドライバ
NXP社のPCA9685を使用した16×12-bit PWM制御基板です。I2Cで通信&制御できます。Aliexpressで購入。Adafruitのドライバが使用できます。

![PCA9685.jpg](https://qiita-image-store.s3.amazonaws.com/0/211703/671ae091-cc5f-28c8-110d-69698c3157fc.jpeg)
##USBカメラ
手持ちのドライブレコーダを使用しました。USBで接続するとPCモードになり、ドライバ無しでUSBカメラとして機能しました。
##3.5inch LCD
ラズパイ用として販売されている3.5inch LCD、タッチパネル付きです。26ピンコネクタが付いており、ラズパイのGPIOピンに差せばモニタ兼タッチパネルとして動作してくれます。ラズパイとはSPI通信しています。

![LCD.jpg](https://qiita-image-store.s3.amazonaws.com/0/211703/66f5c067-012d-450d-e55c-e83749abc662.jpeg)

#下準備1 - まずは顔認識してみる
[RaspberryPiとOpenCVで顔認識してパフォーマンスも改善してみる](http://takuseno.hatenablog.com/entry/2016/09/26/230512)
こちらを参考にしています。
takusenoさま、ありがとうございました。

##Setup
OpenCVのライブラリをインストールします。下記shellにて実行しました。
```Shell
sudo apt-get install libopencv-dev
sudo apt-get install python-opencv
wget http://eclecti.cc/files/2008/03/haarcascade_frontalface_alt.xml
```
##OpenCVで顔認識してみる
USBCAMを接続します。接続するだけです。
##実行
下記Pythonコードを保存します。

```Python:usbcam.py
import cv2.cv as cv
import cv2

cv.NamedWindow("camera", 1)

capture = cv2.VideoCapture(0)
faceCascade = cv2.CascadeClassifier('haarcascade_frontalface_alt.xml')

while True:
    _, img = capture.read()
    img = cv2.resize(img, (320, 240))
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    faces = faceCascade.detectMultiScale(
        gray,
        scaleFactor=1.1,
        minNeighbors=3,
        minSize=(30, 30),
        flags=cv.CV_HAAR_SCALE_IMAGE
    )
    for (x, y, w, h) in faces:
        cv2.rectangle(img, (x, y), (x+w, y+h), (0, 255, 0), 2)
    cv2.imshow("camera", img)
    if cv.WaitKey(10) > 0:
        break
cv.DestroyAllWindows()
```

##実行します。
LCD上でターミナルを起動し、下記実行します。

```Shell
sudo python usbcam.py
```
![DSD_1638.JPG](https://qiita-image-store.s3.amazonaws.com/0/211703/1315fd9c-f38b-6362-6c0d-2942b304a109.jpeg)

![DSD_1644.JPG](https://qiita-image-store.s3.amazonaws.com/0/211703/581648e7-e4c5-b6e1-5f0b-786fbb501e0c.jpeg)

認識してくれました。緑枠が認識された顔です。


#下準備2 - サーボ駆動してみる
##接続
ラズパイとPCA9685基板の接続は下記のとおり。

| Raspberry Pi | -- | PCA9685     |
|:------------:|:--:|:-----------:|
| PIN3(SDA)  | --  | SCA  |
| PIN4(+5V)  | --  | VCC  |
| PIN5(SDL)  | --  | SCL  |
| PIN6(GND)  | --  | GND  |
**注意**:PCA9685の"V+"ではなく**"VCC"**に接続すること。"VCC"はIC用の電源。"V+"はサーボ駆動用電源、そのためこちらは電流の取れる電源を別途供給必要。
###その他設定動作確認は下記先人の偉業を参考にしました
[Raspberry Pi 3でPCA9685を使う](https://qiita.com/ttyokoyama/items/935b51128d2d5bb26f15)
@ttyokoyamaさま、ありがとうございます!

##サンプルプログラム
下記実行し、サーボが動作することを確認します。ch0が水平、ch1が垂直用のサーボで、3秒ごとに移動します。
こちらはUSBカメラからの画像を表示する必要がない為、sshにてリモートから実行しても問題なく動作します。

```Python:servo.py
#!/usr/bin/python

import RPi.GPIO as GPIO
import time
import Adafruit_PCA9685

GPIO.setmode(GPIO.BOARD)


pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(60)

sw1 = 0
sw2 = 0
try:
    while True:
        time.sleep(3)

        if sw1 == 1:
            pwm.set_pwm(1, 0, 600)
            sw1 = 0
        else:
            pwm.set_pwm(1, 0, 375)
            sw1 = 1

        if sw2 == 1:
            pwm.set_pwm(0, 0, 600)
            sw2 = 0
        else:
            pwm.set_pwm(0, 0, 375)
            sw2 = 1


except KeyboardInterrupt:
    pass
```

動作OK
![DSD_1998.JPG](https://qiita-image-store.s3.amazonaws.com/0/211703/57ab3eb4-a2f0-1991-1c7b-7a0c8d6f2e8a.jpeg)

![DSD_1999.JPG](https://qiita-image-store.s3.amazonaws.com/0/211703/4befa3c0-547c-d581-b0ea-5361e018d9ed.jpeg)



#顔認識とサーボ駆動を連携してみる
##i2C配線の取り出し
###下調べ
i2Cの信号ピンは3番5番ピンなのですが、LCDのコネクタで塞がっています。
[3.5 Inch 480x320 TFT Display with Touch Screen for Raspberry Pi](https://www.elecrow.com/wiki/index.php?title=3.5_Inch_480x320_TFT_Display_with_Touch_Screen_for_Raspberry_Pi)
接続を調べてみると、i2Cで仕様する3番5番ピンはNCとなっていましたので、遠慮無く当該のPINに接続することにします。
![Example1567.jpg](https://qiita-image-store.s3.amazonaws.com/0/211703/a27f2aba-e125-e283-a8ae-2978a3c73e92.jpeg)

###信号の取り出し
PCBとLCDは両面テープでやっつけられているだけのようです。
![DSD_1984.JPG](https://qiita-image-store.s3.amazonaws.com/0/211703/4cc2b0df-01e9-e664-3d76-65d965ce1e3d.jpeg)

フレキシブルケーブルを痛めないよう注意しながら、両面テープを剥し、分離させます。
![DSD_1986.JPG](https://qiita-image-store.s3.amazonaws.com/0/211703/4e512a1f-ddcb-50b0-0a85-8a5bdbec3cfc.jpeg)

コネクタのハンダ付け部分に4本のUEW線をハンダ付けします。UEW線はφ0.2mmのものを使用しています。UEW線はポリウレタンで被覆されており、ハンダ付けする箇所のみポリウレタンが溶けハンダ付けされます。このような箇所にハンダ付けするにぴったりな配線です。
![DSD_1989.JPG](https://qiita-image-store.s3.amazonaws.com/0/211703/062951e9-7b60-a986-5d8f-d11ce9e0dbe9.jpeg)

メスコネクタにUEW線をハンダ付けします。ピン番号も書いておきます。4番ピンは+5V線のため、不用意なショートを防ぐため、メスコネクタを使用しておきました。
![DSD_1994.JPG](https://qiita-image-store.s3.amazonaws.com/0/211703/a8f168fd-827b-8a37-3914-1f8fc2777973.jpeg)

液晶が表示されること、i2Cを経由してサーボが動作することを確認してHWの改造は終了です。
![DSD_1995.JPG](https://qiita-image-store.s3.amazonaws.com/0/211703/798c01ea-be18-cb2d-d82d-c385a9045fea.jpeg)


##顔認識させてサーボを動作させる
いよいよ本番です。顔認識した位置にサーボで雲台を向けてみます。

pwm.set_pwmにセットする値と角度の関係は
**150～650 : 0 ～ 180deg**
とのことです。画角等に応じて**pwm.set_pwm**に渡す引数を変更ください。

**LCD上**でターミナルを起動し、下記実行します。

```Shell
sudo python sercam.py
```

```Python:sercam.py
import cv2.cv as cv
import cv2

#servo
import RPi.GPIO as GPIO
import time
import Adafruit_PCA9685

pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(60)


GPIO.setmode(GPIO.BOARD)

cv.NamedWindow("camera", 1)

capture = cv2.VideoCapture(0)
faceCascade = cv2.CascadeClassifier('haarcascade_frontalface_alt.xml')

while True:
    _, img = capture.read()
    img = cv2.resize(img, (320, 240))
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    faces = faceCascade.detectMultiScale(
        gray,
        scaleFactor=1.1,
        minNeighbors=3,
        minSize=(30, 30),
        flags=cv.CV_HAAR_SCALE_IMAGE
    )
    for (x, y, w, h) in faces:
        cv2.rectangle(img, (x, y), (x+w, y+h), (0, 255, 0), 2)
        #pulse = 150～650 : 0 ～ 180deg
        pwm.set_pwm(0, 0, 400 - (x+w)/2)	# h
        pwm.set_pwm(1, 0, 400 - (y+h)/2)	# v
    cv2.imshow("camera", img)
    if cv.WaitKey(10) > 0:
        break
cv.DestroyAllWindows()
```

#動いたぞ!
Googleから顔写真を探してきて、写真をモニタ上で移動してみた。実際には写真の位置を移動させてから8秒ほどタイムラグが有ったすえに検出、サーボ動作している。反応が悪いが昔のラズパイだということで納得はしている。
![move.gif](https://qiita-image-store.s3.amazonaws.com/0/211703/7095b0c4-846a-f1f2-bf16-acefdd5f11de.gif)![undefined]()

#Special thanks!
参考にしたサイトです。ありがとうございます!
[Raspberry Pi 3でPCA9685を使う](https://qiita.com/ttyokoyama/items/935b51128d2d5bb26f15)
[RaspberryPiとOpenCVで顔認識してパフォーマンスも改善してみる](http://takuseno.hatenablog.com/entry/2016/09/26/230512)
[3.5 Inch 480x320 TFT Display with Touch Screen for Raspberry Pi](https://www.elecrow.com/wiki/index.php?title=3.5_Inch_480x320_TFT_Display_with_Touch_Screen_for_Raspberry_Pi)

2018/01/28 Ikeda