顔検出デモでRoverCを動かす

#はじめに
[前回の記事](https://qiita.com/airpocket/items/714aac02b132f486eb67)のあと、Rover制御とfind blobs周りをチューニングしたらきびきび動く様になりました。
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">機械学習の学習に煮詰まったので息抜きにチューニング。<br>広角レンズで視野を広げ、動作パターンをいじって、カメラ設定かえたらだいぶ追従性上がってザクとシャアザクぐらい変わった。<br>スマホレンズを使うのは、nnnさんのアイデアをお借りしましたmm <a href="https://t.co/elp9behRHR">pic.twitter.com/elp9behRHR</a></p>&mdash; AIRPOCKET＠rastaman vibration (@AirpocketRobot) <a href="https://twitter.com/AirpocketRobot/status/1210925043233910785?ref_src=twsrc%5Etfw">December 28, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

しかし、この動作はOpenMVのfind_blobsという機能で画面内の色を検出しているだけで、いわゆるAIを使った物体検出ではありませんでした。

せっかくエッジAIチップたるK210を搭載しているのですから、AIを使って追いかけっこをさせたい！
ということで、物体検出機能をAI処理に変更してみました。

#方法

###マシン構成
マシン構成は、前回と同じくM5StickC + RoverC + UnitVです。RoverCの制御は概ね次の流れで進行しています。

　１．UnitV:カメラで画像取得、ターゲットを発見したら画像内でのX方向の位置と見かけの大きさをStickCに報告。
　２．StickC：UnitVからの情報をもとに、移動方向を決定し前後進、左右並進、左右回転の速度を決めてRoverCに指令。
　３．RoverC：StickC様のいいなり。Stick様の言う通り動けるように、ホイールの回転方向を制御。

今回は１．の部分のターゲット認識をAIで行います。

###どんなAI?

AIと言ってもいろいろですが、学習からやるのは荷が重いのでdemoとして提供されているface detectionの学習済モデルを流用し、人の顔を認識させ追いかけさせることにしました。

#顔認識の準備

###事前準備
UnitVを動かすには、[ファームウェアのアップデート](https://qiita.com/airpocket/items/714aac02b132f486eb67#%E3%83%95%E3%82%A1%E3%83%BC%E3%83%A0%E3%82%A6%E3%82%A7%E3%82%A2%E3%82%92unitv%E3%81%AB%E6%9B%B8%E3%81%8D%E3%81%93%E3%82%80)やプログラムのエディタとなるMaixPy IDEのインストールなどが必要です。前回の記事を参考にしてください。（あまり詳しくは書いていませんが）
また、M5StickVについてもRover制御用コードを書きこんでおきます。前回記事の通りでOKです。

###学習済モデルのインストール
今回はUnitVのdemo用学習済モデルを使用します。私も勉強中なので詳しく説明できませんが、画像を喰わせるとターゲットの座標を吐きだしてくれる賢い奴です。AIの神髄はこの学習済モデルを如何に上手に作るかとい点にあるようですが、私にはまだ難しいので今後の課題としておきます。

UnitVやM5StickVで学習済モデルを使う方法は他にも多くの記事が有りますので詳細は割愛しますが、次の記事を参考に学習済モデル'facedetect.kmodel'をUnitVのメモリアドレス'0x280000'番地に書き込みましょう。書きこむには'K-flash.gui'というツールを用いると簡単です。

お世話になってます。@aNo研 nnnさんの記事：[M5StickVのMaixPyにdemoデータを全部入れる](https://qiita.com/nnn112358/items/8e8c42626c2e7275528e)

#顔認識結果をM5StickCに送るコード

face detectionデモとRoverCデモのコードを合体させただけです。
プログラムは素人なのでお作法とかテクニック皆無なので公開するのはとても恥ずかしいのですが、こんなコードで動きました。

```python
import sensor
import image
import lcd
import time
import utime
import KPU as kpu
from machine import UART
from Maix import GPIO
from fpioa_manager import *

fm.register(34,fm.fpioa.UART1_TX)
fm.register(35,fm.fpioa.UART1_RX)
uart_out = UART(UART.UART1, 115200,8,None,1, timeout=1000, read_buf_len=4096)

#lcd.init()                          UnitVはLCDがついてないのでコメントアウト
#lcd.direction(lcd.YX_LRUD)          UnitVは（略
sensor.reset()
#sensor.set_auto_gain(0)             顔認識は色の影響を受けにくいのでauto_gainはonの方が良さそうです。
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_hmirror(0)
sensor.run(1)

while False:
    uart_out.write('TEST\n')
    utime.sleep_ms(100)

#顔認識起動
task = kpu.load(0x280000)             #アドレス0x280000に書きこんだ学習済みモデルを利用する
anchor = (1.889, 2.5245, 2.9465, 3.94056, 3.99987, 5.3658, 5.155437, 6.92275, 6.718375, 9.01025)
a = kpu.init_yolo2(task, 0.5, 0.3, 5, anchor)

while(True):
    img = sensor.snapshot()
    code = kpu.run_yolo2(task, img)   #画像を喰わせて顔認識結果取得
    #print(code)                      #認識状況確認用
    if code:
        max_area = 0
        target = code[0]
        for i in code:                #複数の顔を認識した場合は一番顔の大きい人（近いとは限らない）をtargetに選択
            if i.w() * i.h() > max_area:
                max_area = int(i.w() * i.h())
                target = i
        #print(target)                       #選択したtargetの確認用
        print(target.w() * target.h())       #選択した（略
        a = img.draw_rectangle(i.rect())
        if uart_out.read(4096):              #targetの情報をStickCに送る処理
            area = max_area
            dx = int(160-(target.x()+(target.w()/2)))   #targetのセンター座標を計算。画像中央が0、左端が+160、右端が-160
            #print(dx)                                  #targetのx座標情報確認用
            hexlist = [(dx >> 8) & 0xFF, dx & 0xFF, (area >> 16) & 0xFF, (area >> 8) & 0xFF, area & 0xFF]
            uart_out.write(bytes(hexlist))
        else:
            pass
    else:
        if uart_out.read(4096):
            hexlist = [0x80, 0x00, 0x00, 0x00, 0x00]
            uart_out.write(bytes(hexlist))
        else:
            pass

    a = lcd.display(img)
a = kpu.deinit(task)
```

#実装結果
実際にface detectモデルを使って検出及び追跡させた結果が次のtweetです。
暗くて見えにくくて済みません。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">UnitVのエッジAI端末としての面目躍如！！<br>UnitV+M5stickC+RoverCのオブジェクト検出方法を顔検出に変更できた。<br>動画ではスマホにおっさんの顔写真を映して、UnitVに検出させてる。<br>従来のimg.find_blobs()による色検出よりも時間がかかり、検出精度も低いので、Rover側も慎重な動作に改めている。 <a href="https://t.co/Uug2mj3vDb">pic.twitter.com/Uug2mj3vDb</a></p>&mdash; AIRPOCKET＠rastaman vibration (@AirpocketRobot) <a href="https://twitter.com/AirpocketRobot/status/1212394782359609344?ref_src=twsrc%5Etfw">January 1, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>
---
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/496179/47c801e3-679c-1eba-8848-db5d45714773.png)
（おっさんの顔写真じゃなくて、みんな大好きlena姉さん使えばよかった(--;)）

---

予定では色認識のfind blobsをface detectモデルに変更するだけですいすい追跡してくれるはずでしたが、そうそううまい話があるわけも無く。。。

残念ながら顔認識は色認識ほど認識精度が良く無く、一度見つけた顔もすぐ見失ってしまいます。（色認識では、ターゲットを、見つけやすい自発光オブジェクトにしたチート仕様にしたことも一因ですが。。。）

tweetの例では、すぐに見失ってしまう理由として次の仮説を立て、Roverの動作パターンを修正しています。

１．顔認識の精度が悪い・・・学習モデルの改良は（私には）難しい。
２．顔認識の速度が遅い・・・そんなの改良できない。（カメラを変えるとちょっとは早くなる）
３．移動により画像が流れたり振動でブレる・・・Roverをゆっくり動かす。時々停止して画像のブレを無くす。見失ったらその場で立ち止まってもう一度良く見る。

また、色検出の際は、カメラに広角レンズを取り付ける事で視認範囲を広げる事が出来ましたが、顔検出の際は逆効果です。広角レンズはマイナス倍率となって、同じ距離のターゲットでもより小さく見えてしまいます。画素数の低下は検出可能距離の低下につながりますので、ov2640の標準レンズの方が認識率があがります。

#M5StickCのコード改造

前項でご紹介した通り、顔検出の学習済みモデルではfind blobsほど精度よく（連続的に、安定して）検出し続けてくれませんでした。細かい考察は後にして、とにかく顔を追いかけさせたい、おもちゃは動かしてナンボ。と言う事で、M5StickCのコードを次のように修正しています。

修正部分はloop()内のみです。

```c++
#include <M5StickC.h>

HardwareSerial VSerial(1);

typedef struct
{
    int16_t dx;
    uint32_t area;
}v_response_t;


uint8_t I2CWrite1Byte(uint8_t Addr, uint8_t Data)
{
    Wire.beginTransmission(0x38);
    Wire.write(Addr);
    Wire.write(Data);
    return Wire.endTransmission();
}

uint8_t I2CWritebuff(uint8_t Addr, uint8_t *Data, uint16_t Length)
{
    Wire.beginTransmission(0x38);
    Wire.write(Addr);
    for (int i = 0; i < Length; i++)
    {
        Wire.write(Data[i]);
    }
    return Wire.endTransmission();
}

uint8_t Setspeed(int16_t Vtx, int16_t Vty, int16_t Wt)
{
    int16_t speed_buff[4] = {0};
    int8_t speed_sendbuff[4] = {0};

    Wt = (Wt > 100) ? 100 : Wt;
    Wt = (Wt < -100) ? -100 : Wt;

    Vtx = (Vtx > 100) ? 100 : Vtx;
    Vtx = (Vtx < -100) ? -100 : Vtx;
    Vty = (Vty > 100) ? 100 : Vty;
    Vty = (Vty < -100) ? -100 : Vty;

    Vtx = (Wt != 0) ? Vtx * (100 - abs(Wt)) / 100 : Vtx;
    Vty = (Wt != 0) ? Vty * (100 - abs(Wt)) / 100 : Vty;

    speed_buff[0] = Vty - Vtx - Wt;
    speed_buff[1] = Vty + Vtx + Wt;
    speed_buff[3] = Vty - Vtx + Wt;
    speed_buff[2] = Vty + Vtx - Wt;

    for (int i = 0; i < 4; i++)
    {
        speed_buff[i] = (speed_buff[i] > 100) ? 100 : speed_buff[i];
        speed_buff[i] = (speed_buff[i] < -100) ? -100 : speed_buff[i];
        speed_sendbuff[i] = speed_buff[i];
    }
    return I2CWritebuff(0x00, (uint8_t *)speed_sendbuff, 4);
}

void setup()
{
    M5.begin();
    M5.Lcd.setRotation(3);
    M5.Lcd.fillScreen(RED);

    VSerial.begin(115200, SERIAL_8N1, 33, 32);
    Wire.begin(0, 26);
}

enum
{
    kNoTarget = 0,
    kLeft,
    kRight,
    kStraight,
    kTooClose
};

const uint16_t kThreshold = 40; // If target is in range ±kThreshold, the car will go straight defo = 20
v_response_t v_data;    // Data read back from V
uint8_t state = 0;  // Car's movement status
int rotate = 1;     //回転方向指定
int dx_old = 0;     //ターゲットの過去位置

void loop()
{
    VSerial.write(0xAF);
    double vpara = 0.4;              //速度一括変更
    int movetime = 150;              //探索時のローテーション運動の継続時間(msec)。movetime(msec)後waitcam(msec)停止
    int waitcam = 150;               //カメラ安定のための停止時間(msec)

    
    if(VSerial.available())
    {
        uint8_t buffer[5];

        VSerial.readBytes(buffer, 5);
        v_data.dx = (buffer[0] << 8) | buffer[1];
        v_data.area = (buffer[2] << 16) | (buffer[3] << 8) | buffer[4];
        
        if(v_data.dx > -160 && v_data.dx < 160)
        {
            M5.Lcd.fillScreen(GREEN);         //ターゲットを認識している間はLCD背景をグリーン表示
            if(v_data.area > 15000)           //オブジェクト領域の面積の閾値　面積が閾値以上だと十分近づいたと判断して停止。
            {
                state = kTooClose;  // Stop
                M5.Lcd.fillScreen(BLUE);      //ターゲットに十分接近したらLCD背景をブルー表示
            }
            else if(v_data.dx > -kThreshold && v_data.dx < kThreshold)
            {
                state = kStraight;  // Go straight
                M5.Lcd.drawString("detect Straight",0,0);  //動作状況確認のためステータス表示を追加
                M5.Lcd.drawNumber(v_data.dx,0,15);
                M5.Lcd.drawNumber(v_data.area,0,30);
            }

            else if(v_data.dx <= -kThreshold)
            {
                state = kRight;  // Go Right
                M5.Lcd.drawString("detect Right",0,0);
                M5.Lcd.drawNumber(v_data.dx,0,15);
                M5.Lcd.drawNumber(v_data.area,0,30);
            }

            else if(v_data.dx >= kThreshold)
            {
                state = kLeft; // Go Left
                M5.Lcd.drawString("detect Left",0,0);
                M5.Lcd.drawNumber(v_data.dx,0,15);
                M5.Lcd.drawNumber(v_data.area,0,30);
            }

            else
            {
                M5.Lcd.drawString("detect Out of range",0,15);
                M5.Lcd.drawNumber(v_data.dx,0,15);
                M5.Lcd.drawNumber(v_data.area,0,30);
                state = kNoTarget;  // Rotate
            }
            
            if (v_data.dx > 0)                 //回転方向を最後にターゲットを見つけた方向に変更する。
            {
                rotate = 1; 
            }
            else
            {
                rotate = -1;
            }
        }
        else
        {
            M5.Lcd.fillScreen(RED);　　　　　　　　　　　　　　　　//ターゲットを見失ったらLCD背景を赤表示
            if(dx_old < 160 && dx_old > -160)                   //ターゲットたった今見失ったら1秒停止（その間にブレの無い画像を取得して再判定する。
            {
                M5.Lcd.drawString("Just lost, waiting",0,0);
                delay(1000);
            }
            else                                                //ターゲットを見失っている間はその場で回転して探索。
            {
                state = kNoTarget;  // Rotate
                M5.Lcd.drawString("NO target",0,0);
            }

        }
        //Serial.printf("%d, %d,%d, %d\n", v_data.dx, dx_old,v_data.area, state);
        dx_old = v_data.dx;
    }

    switch(state)
    {
        case kNoTarget:
            Setspeed(0, 0, 40 * rotate * vpara);   //vparaで速度一括変更
            delay(movetime);                       //movetime(msec)回転を継続
            Setspeed(0, 0, 0);
            delay(waitcam);                        //waitcam(msec)カメラの安定待ち時間
        break;

        case kLeft:
            Setspeed(40 * vpara, 0, 0);            //vparaで速度一括変更
            delay(20);
            Setspeed(0, 0, 0);
        break;

        case kRight:
            Setspeed(-40 * vpara, 0, 0);           //vparaで速度一括変更
            delay(20);
            Setspeed(0, 0, 0);
        break;

        case kStraight:
            Setspeed(0, 40 * vpara, 0);            //vparaで速度一括変更
            delay(20);
            Setspeed(0, 0, 0);
        break;

        case kTooClose:
            Setspeed(0, 0, 0);
        break;

    }
}
```
#まとめ
これで念願の初AIが達成できました。
モジュール使ってるだけなのでAI搭載デジカメ使ってるのと大差ないかもしれませんが、AIしてる実感は得られました。
次の目標はオリジナルの学習済モデル作成かなぁ。