ESP32で顔検出

# 1. はじめに
Arduino core for the ESP32のライブラリには標準でCamera web serverがサンプルコードで入っています．サンプルコードはこちらです．
Arduino-IDEでボードとしてESP32関連を選択しておき，下記を選びます．
ファイル → スケッチ例 → ESP32 → Camera → CameraWebServer
このコードには，face_detectionやface_recognitionなどの関数を呼び出しています．ESP32ライブラリに，顔検出や顔認識も含まれています．これらを用いるだけで顔検出ができます．今回はface_detectionの呼び出し方について解説します．

# 2. 使い方
ここでは使い方の概要を説明します．(サンプルコード全体は後で示します．）

include

```c++
#include "fd_forward.h"
```

呼び出し部分

```c++
// imageSource : RGB888フォーマットの画像
// widthImage, heightImage , lengthImage : 画像の 幅 高さ 長さ（=幅*高さ*3)
dl_matrix3du_t *image_matrix = dl_matrix3du_alloc(1, widthImage, heightImage, 3);
memcopy(image_matrix->item, imageSource,lengthImage);

mtmn_config_t mtmn_config = mtmn_init_config();
box_array_t *net_boxes = face_detect(image_matrix, &mtmn_config);
```


# 3. 解説

fd_forward.h は，packages/esp32/hardware/esp32/1.0.4/tools/sdk/include/esp-face にあります．このフォルダは，OSによって異なり，windowsでは，c:\Users\ユーザ名\AppData\Local\Arduino15\ で，macでは，~/Library/Arduino15/で，Raspberry Pi OSでは，~/.arduino15/ にあります．(Arduino-IDE 1.8.13時点)

これらのファイルは，arduinoのボードマネージャでESP32をインストールすると一緒に入ります． face_detectionの実体は，packages/esp32/hardware/esp32/1.0.4/tools/sdk/lib/libfd.a です． ソースは， https://github.com/espressif/esp-face/tree/master/face_detection にあります．
fd_forward.hで，face_detectに関する部分を見てみましょう．

```c++
    /**
     * @brief Do MTMN face detection, return box and landmark infomation.
     * 
     * @param image_matrix      Image matrix, rgb888 format
     * @param config            Configuration of MTMN i.e. score threshold, nms threshold, candidate number threshold, pyramid, min face size
     * @return box_array_t*     A list of boxes and score.
     */
    box_array_t *face_detect(dl_matrix3du_t *image_matrix,
                             mtmn_config_t *config);
```

返りは，box_array_t です．
第一引数は，画像の構造体 dl_matrix3du_t で，第二引数は，パラメータの構造体 mtmn_config_t です．
それぞれ設定が必要です．

画像の構造体は，次のように画像の幅と高さで定義します．固定値の1と3は，そのまま利用すると良いと思います．3は，RGBの3チャネルの意味です．


```c++

dl_matrix3du_t *image_matrix = dl_matrix3du_alloc(1, widthImage, heightImage, 3);
```

これらの定義を fd_forward.h に記載があります．

```c++

/*
 * @brief Allocate a 3D matrix with 8-bits items, the access sequence is NHWC
 *
 * @param n     Number of matrix3d, for filters it is out channels, for others it is 1
 * @param w     Width of matrix3d
 * @param h     Height of matrix3d
 * @param c     Channel of matrix3d
 * @return      3d matrix
 */
dl_matrix3du_t *dl_matrix3du_alloc(int n, int w, int h, int c);
```

```c++
typedef struct
{
    int w;          /*!< Width */
    int h;          /*!< Height */
    int c;          /*!< Channel */
    int n;          /*!< Number of filter, input and output must be 1 */
    int stride;     /*!< Step between lines */
    uc_t *item;     /*!< Data */
} dl_matrix3du_t;
```


パラメータは次のように定義します．

```c++
mtmn_config_t mtmn_config = mtmn_init_config();
```

初期値を設定する関数は fd_forward.h にあり，これを利用しています．

```c++
    static inline mtmn_config_t mtmn_init_config()
    {
        mtmn_config_t mtmn_config;
        mtmn_config.type = FAST;
        mtmn_config.min_face = 80;
        mtmn_config.pyramid = 0.707;
        mtmn_config.pyramid_times = 4;
        mtmn_config.p_threshold.score = 0.6;
        mtmn_config.p_threshold.nms = 0.7;
        mtmn_config.p_threshold.candidate_number = 20;
        mtmn_config.r_threshold.score = 0.7;
        mtmn_config.r_threshold.nms = 0.7;
        mtmn_config.r_threshold.candidate_number = 10;
        mtmn_config.o_threshold.score = 0.7;
        mtmn_config.o_threshold.nms = 0.7;
        mtmn_config.o_threshold.candidate_number = 1;

        return mtmn_config;
    }
```

なお，face_detect内から呼ばれる計算は，下記にあるようです．これ以降の説明は割愛します．
https://github.com/espressif/esp-face/blob/master/image_util/image_util.c


# 4. 入力画像について

face_detectで利用する画像は，RGB888です．つまり，24bitビットマップと同じです．
その画像データの入力方法はいくつか考えられます．

1. カメラモジュールで撮影（カメラモジュールの接続が必要）
2. プログラムに埋め込む
3. SDメモリカードから読み込む（SD接続が必要）
4. Wi-Fiを介して取得

上記2以降は，プログラムのデバッグなど，再現性の確認には有効ですが，あえてESP32で行うメリットは無いかもしれません．
今回は上記1について説明します．これは，M5CameraやESP32Camなどの前提になります．ここからは，[M5cameraでhttp postによる画像送信](https://qiita.com/dzonesasaki/items/88deb9a2acb8be2d7cfb)の内容と重複します．

esp_camera.hを使い，初期設定後，次の1行で画像取得します．

```c++
 camera_fb_t * fb = esp_camera_fb_get();
```

しかし，今回は初期設定によって大きく異なります．まずは，カメラから取得する画像のフォーマットです．
普段は次のようにJPEGで取得するのがほとんどだと思います．

```c++
config.pixel_format = PIXFORMAT_JPEG; 
```
JPEGで取得するメリットは，データ量が少なくて済む点です．そして，転送が失敗しにくいメリットもあります．デメリットは，画質の劣化です．これは避けられません．ですが，経験的にJPEG取得がおすすめです．

JPEGで取得した場合，face_detectionに入れるためにRGB888に変換する必要があります．

まずは，画像のサイズの取得が必要です．face_detectionでは，QVGAかQQVGAのサイズが推奨されています．初期設定でサイズを次のようにQVGAとしましょう．

```c++
config.frame_size = FRAMESIZE_QVGA; 
```
カメラフレームバッファ fbから幅と高さを取得します．ついでに，データの長さも計算しましょう．ここで，3というのはRGBの3チャネルという意味です．グレースケールであれば1チャネルです．

```c++
uint32_t widthImage = fb->width;
uint32_t heightImage = fb->height;
uint32_t lengthImage = widthImage * heightImage * 3;
```

準備が整いましたので，次のようにfmt2rgb888でJPEGからRGB888に変換します．

```c++
dl_matrix3du_t *image_matrix = dl_matrix3du_alloc(1, widthImage, heightImage, 3);
uint8_t *out_buf = image_matrix->item;
bool bFlug888Conv = fmt2rgb888(fb->buf, lengthImage , fb->format, out_buf);
```
単に変換するだけであれば，格納先はuint8_tでmallocしたポインタでも良いのですが，face_detectionでは，画像データをdl_matrix3du_tの構造で格納したものを入力しますので，ライブラリで用意されたdl_matrix3du_allocを使います．fmt2rgb888の返値は成否です．

ここまでくると，顔検出の準備が整いました．前述の通り，次の2行で実行できます．

```c++
mtmn_config_t mtmn_config = mtmn_init_config();
box_array_t *net_boxes = face_detect(image_matrix, &mtmn_config);
```
返値のbox_array_tは，検出時のパラメータです．NULLであれば，その画像には顔が無かったということになります．
検出された場合は，顔の周囲を四角で囲った絵を描いて，それを出力します．

# 5. 検出結果の描画と出力

顔の検出結果の描画関数は，サンプルコードCameraWebServerから拝借します．下記はそれをコピペしたものです．
コードを見ると，box_array_tの利用方法が判ると思います．

```c++
#include "fb_gfx.h"

#define FACE_COLOR_WHITE  0x00FFFFFF
#define FACE_COLOR_BLACK  0x00000000
#define FACE_COLOR_RED    0x000000FF
#define FACE_COLOR_GREEN  0x0000FF00
#define FACE_COLOR_BLUE   0x00FF0000
#define FACE_COLOR_YELLOW (FACE_COLOR_RED | FACE_COLOR_GREEN)
#define FACE_COLOR_CYAN   (FACE_COLOR_BLUE | FACE_COLOR_GREEN)
#define FACE_COLOR_PURPLE (FACE_COLOR_BLUE | FACE_COLOR_RED)

void draw_face_boxes(dl_matrix3du_t *image_matrix, box_array_t *boxes, int face_id){
    int x, y, w, h, i;
    uint32_t color = FACE_COLOR_YELLOW;
    if(face_id < 0){
        color = FACE_COLOR_RED;
    } else if(face_id > 0){
        color = FACE_COLOR_GREEN;
    }
    fb_data_t fb;
    fb.width = image_matrix->w;
    fb.height = image_matrix->h;
    fb.data = image_matrix->item;
    fb.bytes_per_pixel = 3;
    fb.format = FB_BGR888;
    for (i = 0; i < boxes->len; i++){
        // rectangle box
        x = (int)boxes->box[i].box_p[0];
        y = (int)boxes->box[i].box_p[1];
        w = (int)boxes->box[i].box_p[2] - x + 1;
        h = (int)boxes->box[i].box_p[3] - y + 1;
        fb_gfx_drawFastHLine(&fb, x, y, w, color);
        fb_gfx_drawFastHLine(&fb, x, y+h-1, w, color);
        fb_gfx_drawFastVLine(&fb, x, y, h, color);
        fb_gfx_drawFastVLine(&fb, x+w-1, y, h, color);
#if 0
        // landmark
        int x0, y0, j;
        for (j = 0; j < 10; j+=2) {
            x0 = (int)boxes->landmark[i].landmark_p[j];
            y0 = (int)boxes->landmark[i].landmark_p[j+1];
            fb_gfx_fillRect(&fb, x0, y0, 3, 3, color);
        }
#endif
    }
}
```

呼び出しは次のように行います．NULLでなければ，draw_face_boxesで顔に枠を描きます．image_matrixを上書きします．
face_idで色づけを変えているのは，顔認識プログラムでの結果を反映させるためだと思いますが，今回は利用しません．
サンプルコードCameraWebServerでは，このif文の中にメモリ解放が記述されていますが，これを行うと，リブートしてしまいます．注意ポイントとして，コメントアウトしたものをあえて記載しました．

組込で使う場合は，別のアクションをif文の中に書き加えても良いと思います．

```c++
int face_id =0;
if (net_boxes){
  draw_face_boxes(image_matrix, net_boxes, face_id);
//  free(net_boxes->score);
//  free(net_boxes->box);
//  free(net_boxes->landmark);
//  free(net_boxes);
}
```
これで，一通りの処理が終わりました．この画像を出力したい場合，ビットマップでも良いですが，サイズが大きくなりますので，jpegが便利だと思います．次のようにfmt2jpgを用いて変換します．

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

```c++
uint8_t * ui8BufJpg;
uint32_t iNumDat;
int quality = 80;
bool bFlugJpegConv = fmt2jpg(image_matrix->item,lengthImage, widthImage, heightImage,PIXFORMAT_RGB888 ,quality , &ui8BufJpg, &iNumDat);
```
生成されたJPEGは．[Arduino HTTPClientでファイルのバイナリ送信](https://qiita.com/dzonesasaki/items/1417b917751443994ffc)の方法などを用いて送信しても良いでしょう．

```c++
httppost( ui8BufJpg , iNumDat);
```


# 6. ソースコード

M5Camera model B用です．配線が異なる場合は適宜修正してください．

<details><summary>facedethttppost.ino</summary><div>

```c++
#include <WiFi.h>
#include <WiFiClient.h> 
#include <esp_camera.h>
#include "fb_gfx.h"
#include "fd_forward.h"
#include <HTTPClient.h>
HTTPClient myHttp;

#define URL1 "http://"
#define URL2 ":1880/mypost"

char *ssidC="SSID";
char *passC="PASS"; 
char *servC="192.168.1.2";//server address

#define STRING_BOUNDARY "123456789000000000000987654321"
#define STRING_MULTIHEAD02 "Content-Disposition: form-data; name=\"uploadFile\"; filename=\"./testfig2.jpg\""
#define STRING_MULTIHEAD03 "Content-Type: image/jpg"

// for m5camera model B
#define PWDN_GPIO_NUM     -1
#define RESET_GPIO_NUM    15
#define XCLK_GPIO_NUM     27
#define SIOD_GPIO_NUM     22 //25
#define SIOC_GPIO_NUM     23

#define Y9_GPIO_NUM       19
#define Y8_GPIO_NUM       36
#define Y7_GPIO_NUM       18
#define Y6_GPIO_NUM       39
#define Y5_GPIO_NUM        5
#define Y4_GPIO_NUM       34
#define Y3_GPIO_NUM       35
#define Y2_GPIO_NUM       32
#define VSYNC_GPIO_NUM    25 //22
#define HREF_GPIO_NUM     26
#define PCLK_GPIO_NUM     21

void initWifiClient(void){
  Serial.print("Connecting to ");
  Serial.println(ssidC);

  WiFi.begin( ssidC, passC);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("WiFi connected.");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
}


void initCam(void){
  // ref to https://www.mgo-tec.com/blog-entry-esp32-ov2640-ssd1331-test1.html/3
  camera_config_t config;
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer = LEDC_TIMER_0;
  config.pin_d0 = Y2_GPIO_NUM;
  config.pin_d1 = Y3_GPIO_NUM;
  config.pin_d2 = Y4_GPIO_NUM;
  config.pin_d3 = Y5_GPIO_NUM;
  config.pin_d4 = Y6_GPIO_NUM;
  config.pin_d5 = Y7_GPIO_NUM;
  config.pin_d6 = Y8_GPIO_NUM;
  config.pin_d7 = Y9_GPIO_NUM;
  config.pin_xclk = XCLK_GPIO_NUM;
  config.pin_pclk = PCLK_GPIO_NUM;
  config.pin_vsync = VSYNC_GPIO_NUM;
  config.pin_href = HREF_GPIO_NUM;
  config.pin_sscb_sda = SIOD_GPIO_NUM;
  config.pin_sscb_scl = SIOC_GPIO_NUM;
  config.pin_pwdn = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;
  config.xclk_freq_hz = 20000000;
  config.pixel_format = PIXFORMAT_JPEG;
  config.jpeg_quality = 10;
  config.frame_size = FRAMESIZE_QVGA;
  config.fb_count = 1;

  esp_err_t err = esp_camera_init(&config);
  if (err != ESP_OK) {
    Serial.printf("Camera init failed with error 0x%x", err);
    while(1){}
    return;
  }
}



int32_t httppost( uint8_t * ui8BufJpg, uint32_t iNumDat ){

  String stMyURL="";
  stMyURL+=URL1;
  stMyURL+=String(servC);
  stMyURL+=URL2;
  myHttp.begin(stMyURL);
    
  String stConType ="";
  stConType +="multipart/form-data; boundary=";
  stConType +=STRING_BOUNDARY;
  myHttp.addHeader("Content-Type", stConType);

  String stMHead="";
  stMHead += "--";
  stMHead += STRING_BOUNDARY;
  stMHead += "\r\n";
  stMHead += STRING_MULTIHEAD02;
  stMHead += "\r\n";
  stMHead += STRING_MULTIHEAD03;
  stMHead += "\r\n";
  stMHead += "\r\n";
  uint32_t iNumMHead = stMHead.length();

  String stMTail="";
  stMTail += "\r\n";
  stMTail += "--";
  stMTail += STRING_BOUNDARY;
  stMTail += "--";
  stMTail += "\r\n";
  stMTail += "\r\n";  
  uint32_t iNumMTail = stMTail.length();

  uint32_t iNumTotalLen = iNumMHead + iNumMTail + iNumDat;

  uint8_t *uiB = (uint8_t *)ps_malloc(sizeof(uint8_t)*iNumTotalLen);

  for(int uilp=0;uilp<iNumMHead;uilp++){
    uiB[0+uilp]=stMHead[uilp];
  }
  for(int uilp=0;uilp<iNumDat;uilp++){
    uiB[iNumMHead+uilp]=ui8BufJpg[uilp];
  }
  for(int uilp=0;uilp<iNumMTail;uilp++){
    uiB[iNumMHead+iNumDat+uilp]=stMTail[uilp];
  }

  int32_t httpResponseCode = (int32_t)myHttp.POST(uiB,iNumTotalLen);
  myHttp.end();
  free(uiB);
  return (httpResponseCode);
}



#define FACE_COLOR_WHITE  0x00FFFFFF
#define FACE_COLOR_BLACK  0x00000000
#define FACE_COLOR_RED    0x000000FF
#define FACE_COLOR_GREEN  0x0000FF00
#define FACE_COLOR_BLUE   0x00FF0000
#define FACE_COLOR_YELLOW (FACE_COLOR_RED | FACE_COLOR_GREEN)
#define FACE_COLOR_CYAN   (FACE_COLOR_BLUE | FACE_COLOR_GREEN)
#define FACE_COLOR_PURPLE (FACE_COLOR_BLUE | FACE_COLOR_RED)

static void draw_face_boxes(dl_matrix3du_t *image_matrix, box_array_t *boxes, int face_id){
    int x, y, w, h, i;
    uint32_t color = FACE_COLOR_YELLOW;
    if(face_id < 0){
        color = FACE_COLOR_RED;
    } else if(face_id > 0){
        color = FACE_COLOR_GREEN;
    }
    fb_data_t fb;
    fb.width = image_matrix->w;
    fb.height = image_matrix->h;
    fb.data = image_matrix->item;
    fb.bytes_per_pixel = 3;
    fb.format = FB_BGR888;
    for (i = 0; i < boxes->len; i++){
        // rectangle box
        x = (int)boxes->box[i].box_p[0];
        y = (int)boxes->box[i].box_p[1];
        w = (int)boxes->box[i].box_p[2] - x + 1;
        h = (int)boxes->box[i].box_p[3] - y + 1;
        fb_gfx_drawFastHLine(&fb, x, y, w, color);
        fb_gfx_drawFastHLine(&fb, x, y+h-1, w, color);
        fb_gfx_drawFastVLine(&fb, x, y, h, color);
        fb_gfx_drawFastVLine(&fb, x+w-1, y, h, color);
#if 0
        // landmark
        int x0, y0, j;
        for (j = 0; j < 10; j+=2) {
            x0 = (int)boxes->landmark[i].landmark_p[j];
            y0 = (int)boxes->landmark[i].landmark_p[j+1];
            fb_gfx_fillRect(&fb, x0, y0, 3, 3, color);
        }
#endif
    }
}

void setup() {
  Serial.begin(115200);
  initWifiClient();
  myHttp.setReuse(true);
  initCam();
}

void loop() {
  camera_fb_t * fb = NULL;
  fb = esp_camera_fb_get();
  if (!fb) {
    Serial.println("failure: Camera capture");
	delay(1000);
    return;
  }

  uint16_t widthImage=fb->width;
  uint16_t heightImage=fb->height;
  int num_components = 3;
  uint32_t lengthImage = widthImage * heightImage * num_components;
  int quality = 80;
  dl_matrix3du_t *image_matrix = dl_matrix3du_alloc(1, widthImage, heightImage, 3);
  uint8_t *out_buf = image_matrix->item;
  bool bFlug888Conv = fmt2rgb888(fb->buf, lengthImage , PIXFORMAT_JPEG, out_buf);

  esp_camera_fb_return(fb);

  if(!bFlug888Conv){
    Serial.println("failure: fmt2rgb888");
	delay(1000);
    return;
  }
  mtmn_config_t mtmn_config = mtmn_init_config();
  box_array_t *net_boxes = face_detect(image_matrix, &mtmn_config);

  int face_id = 0;  
  if (net_boxes){
    draw_face_boxes(image_matrix, net_boxes, face_id);
    Serial.println("done: draw_face_boxes");
//  free(net_boxes->score);
//  free(net_boxes->box);
//  free(net_boxes->landmark);
//  free(net_boxes);
  }
  else{
    Serial.println("no faces");
  }
  
  uint8_t * ui8BufJpg;
  uint32_t iNumDat ;

  bool bFlugJpegConv = fmt2jpg(image_matrix->item,lengthImage, widthImage, heightImage,PIXFORMAT_RGB888 ,quality , &ui8BufJpg, &iNumDat);
  dl_matrix3du_free(image_matrix);
  int iRetHttp = httppost( ui8BufJpg , iNumDat);
  free(ui8BufJpg);
  if (iRetHttp==200)
  {
    Serial.print("*");
  }
  else
  {
    Serial.println("");
    Serial.print("failure: http post. return code: ");
    Serial.println(iRetHttp);
  }

}
```

</div></details>

# 7. むすび

M5Cameraの使い道を考えた際，画像をアップロードするだけだと勿体ない感じがしてました．その際，OpenCVに入っているHaar-Like程度であれば簡単に実装できるだろうと思っていました．しかし，サンプルコードのCameraWebServerを見ていくと，それらを実装するまでも無く，顔検出が既に実装されているということを知り，その使い方を調査しました．
最も苦労したのは，camera_fbの扱いでした．どうもカメラのプロセスが予想以上に複雑で，外部からcamera_fbを操作しようとすると，リブートしてしまいました．今回は紹介してませんが，デバッグではftp_clientを多用しました．処理の途中で配列をftpで吐き出し，意図した形になっているかを確認しながら進めました．ftp_clientも現在ver 0.1.3と，まだ荒削りなため，それを使うだけでもまた難儀したのですが，良い勉強でした．
今回，esp_faceのモジュールを呼び出しているだけですが，そのリポジトリを見ると結構な計算量だと感じます．これを1秒以内に行うというのは，ESP32の凄さでもあり，最適化を行っている方々の力でもあると感じます．