顔認識をする【道具としての OpenCV 画像認識】

# 本記事でできること
こっちを見ているロボットとハンドアイカメラ
![IMG_2031.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/37698/477afc0c-b834-998f-bb37-e416df40871a.jpeg)


# 想定シーン
## ロボットをビジュアルフィードバック制御する
カメラを使ったロボットアームの制御方法を、ビジュアルフィードバック制御といいます。ここでは、ビジュアルフィードバック制御を使った自動化システムを開発する際に必要となる画像認識プログラムをまとめています。

## 顔認識をする
子供の頃、2006年に開催された愛・地球博（愛知県で開催された万博博覧会）に行きました。そこで、サイクロプス（ギリシャ神話のひとつ目の巨人の名前）と名付けられたロボットが展示されていたのが印象的でした。ひとつ目の中にカメラが仕込まれており、来場者の顔を認識して首を動かしてトラッキングする、というものでした。技術的なというよりは、芸術的な展示で、不気味さの中にあるかっこよさに惹かれたのを覚えています。
ということで、本記事では、ロボットの手先にカメラを搭載して、人間の顔を認識してロボットアームを動かして追いかけるプログラムを掲載します。

# 実行環境
- OS：Ubuntu 22.04 LTS
- 言語：C++
- クルーボ：v5.1.0

## クルーボについて
ロボットアプリケーション開発には、株式会社チトセロボティクスのロボット制御ソフトウェア「クルーボ」を使用します。本記事のプログラムは、クルーボがインストールされた制御コンピュータ上で動作します。

- クルーボの製品サイト：https://chitose-robotics.com/

## プログラム抜粋
### 前準備
```c++
cv::CascadeClassifier cascade;  // カスケード分類器格納場所
cascade.load("../sample_data/haarcascade_frontalface_alt.xml");  // 正面顔情報が入っているカスケード
std::vector<cv::Rect> faces;  // 輪郭情報を格納場所
```

### 制御ループ内
```c++
cascade.detectMultiScale(input_image, faces, 1.2, 5, 0, cv::Size(20, 20));
```


## 全体プログラム
```c++
#include <iostream>
#include <optional>
#include <string>

#include <opencv2/aruco.hpp>
#include <opencv2/opencv.hpp>

#include "crewbo/crewbo.h"
#include "project_lib/vision_assistant.h"

int main() {
    // カメラオブジェクトを生成する。
    crewbo::camera::RealSense camera({640, 360});

    cv::CascadeClassifier cascade;  // カスケード分類器格納場所
    cascade.load("../sample_data/haarcascade_frontalface_alt.xml");  // 正面顔情報が入っているカスケード
    std::vector<cv::Rect> faces;  // 輪郭情報を格納場所

    crewbo::robot::melfa::base_kit::MelfaRv4frl robot;
    using namespace crewbo::literals;
    robot.moveJointByAbsJoint_({0_deg, 0_deg, 60_deg, 0_deg, 30_deg, 0_deg}, 0.3f);

    bool is_face_detected = false;
    cv::Point detected_face_point(
            static_cast<int>(camera.getPrincipalPoint_().x), static_cast<int>(camera.getPrincipalPoint_().y));

    {
        crewbo::robot::concurrent_updater::ConcurrentPoseUpdater updater(&robot, 0.2f, 1e-3f);

        // 制御ループを開始する。キーボードのqキーを押下すれば終了できる。
        std::cout << "q キー押下で終了します。 " << std::endl;
        while (!(cv::waitKey(1) == 'q')) {
            const cv::Mat camera_image = camera.fetchSingleFrame_();
            cv::Mat input_image;
            if (is_face_detected == true) {
                const int roi_circle_radius = 70;
                const cv::Mat roi_mask_image =
                        va::get_mask_image_with_circle_roi(camera_image.size(), detected_face_point, roi_circle_radius);
                input_image = va::set_mask_on_image(camera_image, roi_mask_image);
            } else {
                input_image = camera_image.clone();
            }

            cascade.detectMultiScale(input_image, faces, 1.2, 5, 0, cv::Size(20, 20));

            // 検出できなかった場合は、ロボットに指令せずにループ先頭に戻る。
            if (faces.size() <= 0) {
                is_face_detected = false;
                cv::imshow("camera_image", camera_image);
                continue;
            }

            // 検出できた場合。
            cv::Point rect_corner_lefttop(faces[0].x, faces[0].y);
            cv::Point rect_corner_rightbottom(faces[0].x + faces[0].width, faces[0].y + faces[0].height);
            cv::rectangle(camera_image, rect_corner_lefttop, rect_corner_rightbottom, {0, 255, 255}, 1, cv::LINE_4);
            cv::imshow("camera_image", camera_image);

            // 画像内偏差計算
            const float face_center_u = static_cast<float>(faces[0].x + faces[0].width / 2);
            const float face_center_v = static_cast<float>(faces[0].y + faces[0].height / 2);
            const float image_center_u = camera.getPrincipalPoint_().x;
            const float image_center_v = camera.getPrincipalPoint_().y;
            const float error_u = static_cast<float>(face_center_u - image_center_u);
            const float error_v = static_cast<float>(face_center_v - image_center_v);

            // 制御入力構成
            const float error_x = 0.f;
            const float error_y = -error_u;
            const float error_z = -error_v;
            const float error_ra = 0.f;
            const float error_rb = 0.2f * error_v;
            const float error_rc = -0.2f * error_u;
            const crewbo::Degrees error_ra_degree{error_ra};
            const crewbo::Degrees error_rb_degree{error_rb};
            const crewbo::Degrees error_rc_degree{error_rc};

            // ゲイン印加
            const float p_gain_for_position = 0.05f;
            const float input_x = p_gain_for_position * error_x;
            const float input_y = p_gain_for_position * error_y;
            const float input_z = p_gain_for_position * error_z;
            const float p_gain_for_rotation = 0.05f;
            const crewbo::Degrees input_ra = p_gain_for_rotation * error_ra_degree;
            const crewbo::Degrees input_rb = p_gain_for_rotation * error_rb_degree;
            const crewbo::Degrees input_rc = p_gain_for_rotation * error_rc_degree;

            // ロボット動作
            const crewbo::Pose input_rel_pose{input_x, input_y, input_z, input_ra, input_rb, input_rc};
            updater.updateGoalByRelPose_(input_rel_pose);
        }
    }
}
```

## おわりに
人手作業をロボットアームで自動化するために、カメラを使ったロボット制御＝ビジュアルフィードバック制御が大切です。
ロボット制御用の画像認識でも中身のひとつひとつはシンプルなので、要素に分解して解説していきたいと思います。