画像に目印を描く【道具としての OpenCV 画像認識】

# 本記事でできること
目印となる十字マーク（クロス）と矢印を描画した画像を表示します。
![draw_cross.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/37698/dfd4121b-3c58-aecb-047a-98a5403333ff.png)


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

## 画像に目印を描く
ロボットで対象物体が複数ある場合などに画像で認識していることを表現するために目印として十字マークを表示したり、コンベア上を移動する対象物体のベクトルを矢印で表記したりします。本記事では色やサイズを変えて描画できるプログラムを掲載します。

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

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

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

## プログラム抜粋
```c++
cv::Mat draw_cross(
        const cv::Mat image,
        const cv::Point center,
        const float scale,
        const float angle,
        const cv::Scalar color,
        const int thickness) {
    cv::Mat drawed_image = image.clone();
    drawed_image = draw_both_directions_line(drawed_image, center, scale, angle, color, thickness);
    drawed_image = draw_both_directions_line(drawed_image, center, scale, angle + 90.f, color, thickness);
    return drawed_image;
}

cv::Mat draw_arrow(
        const cv::Mat image,
        const cv::Point start,
        const cv::Point finish,
        const cv::Scalar color,
        const int thickness) {
    cv::Mat drawed_image = image.clone();
    cv::arrowedLine(drawed_image, start, finish, color, thickness, cv::LINE_4);
    return drawed_image;
}

cv::Mat draw_arrow(
        const cv::Mat image,
        const cv::Point finish,
        const float length,
        const float angle,
        const cv::Scalar color,
        const int thickness) {
    cv::Point start;
    start.x = finish.x - static_cast<int>(length * cos(DEG2RAD(-angle)));
    start.y = finish.y + static_cast<int>(length * sin(DEG2RAD(-angle)));
    return draw_arrow(image, start, finish, color, thickness);
}
```


## 補足：角度のラジアンへの変換
描画に三角関数を使用するときにラジアン変換が必要になります。よく使うので、お助け関数として覚えておくと便利です。
```c++
float DEG2RAD(const float degree) { return degree * (static_cast<float>(M_PI) / 180.f); }
```

## 補足：両方向への線を描画する
十字マークの表示は線描画機能を繰り返して実装します。始点を指定する方法だけではなく、中点を指定して線を描画する機能についてプログラムを掲載します。

```c++
cv::Mat draw_line(
        const cv::Mat image,
        const cv::Point start,
        const cv::Point finish,
        const cv::Scalar color,
        const int thickness) {
    cv::Mat drawed_image = image.clone();
    cv::line(drawed_image, start, finish, color, thickness, cv::LINE_4);
    return drawed_image;
}

cv::Mat draw_both_directions_line(
        const cv::Mat image,
        const cv::Point center,
        const float length,
        const float angle,
        const cv::Scalar color,
        const int thickness) {
    cv::Point start;
    cv::Point finish;
    start.x = center.x + static_cast<int>(length / 2.f * cos(DEG2RAD(-angle)));
    start.y = center.y - static_cast<int>(length / 2.f * sin(DEG2RAD(-angle)));
    finish.x = center.x - static_cast<int>(length / 2.f * cos(DEG2RAD(-angle)));
    finish.y = center.y + static_cast<int>(length / 2.f * sin(DEG2RAD(-angle)));
    return draw_line(image, start, finish, color, thickness);
}
```

## 全体プログラム
```c++
#include <iostream>
#include <opencv2/opencv.hpp>

float DEG2RAD(const float degree) { return degree * (static_cast<float>(M_PI) / 180.f); }

cv::Mat draw_line(
        const cv::Mat image,
        const cv::Point start,
        const cv::Point finish,
        const cv::Scalar color,
        const int thickness) {
    cv::Mat drawed_image = image.clone();
    cv::line(drawed_image, start, finish, color, thickness, cv::LINE_4);
    return drawed_image;
}

cv::Mat draw_both_directions_line(
        const cv::Mat image,
        const cv::Point center,
        const float length,
        const float angle,
        const cv::Scalar color,
        const int thickness) {
    cv::Point start;
    cv::Point finish;
    start.x = center.x + static_cast<int>(length / 2.f * cos(DEG2RAD(-angle)));
    start.y = center.y - static_cast<int>(length / 2.f * sin(DEG2RAD(-angle)));
    finish.x = center.x - static_cast<int>(length / 2.f * cos(DEG2RAD(-angle)));
    finish.y = center.y + static_cast<int>(length / 2.f * sin(DEG2RAD(-angle)));
    return draw_line(image, start, finish, color, thickness);
}

cv::Mat draw_cross(
        const cv::Mat image,
        const cv::Point center,
        const float scale,
        const float angle,
        const cv::Scalar color,
        const int thickness) {
    cv::Mat drawed_image = image.clone();
    drawed_image = draw_both_directions_line(drawed_image, center, scale, angle, color, thickness);
    drawed_image = draw_both_directions_line(drawed_image, center, scale, angle + 90.f, color, thickness);
    return drawed_image;
}

cv::Mat draw_arrow(
        const cv::Mat image,
        const cv::Point start,
        const cv::Point finish,
        const cv::Scalar color,
        const int thickness) {
    cv::Mat drawed_image = image.clone();
    cv::arrowedLine(drawed_image, start, finish, color, thickness, cv::LINE_4);
    return drawed_image;
}

cv::Mat draw_arrow(
        const cv::Mat image,
        const cv::Point finish,
        const float length,
        const float angle,
        const cv::Scalar color,
        const int thickness) {
    cv::Point start;
    start.x = finish.x - static_cast<int>(length * cos(DEG2RAD(-angle)));
    start.y = finish.y + static_cast<int>(length * sin(DEG2RAD(-angle)));
    return draw_arrow(image, start, finish, color, thickness);
}

int main(void) {
    const cv::Size image_size(1280, 960);
    const cv::Mat black_image = cv::Mat::zeros(image_size.height, image_size.width, CV_8UC3);

    cv::Mat drawed_image = black_image.clone();

    const cv::Point cross_center(200, 300);
    const float cross_scale = 70.f;
    drawed_image = draw_cross(drawed_image, cross_center, cross_scale, 45.f, cv::Scalar(0, 0, 255), 5);

    const cv::Point start_point(100, 100);
    const cv::Point finish_point(900, 600);
    drawed_image = draw_arrow(drawed_image, start_point, finish_point, cv::Scalar(255, 0, 0), 10);

    const float length = 100.f;
    const float angle = 130.f;
    drawed_image = draw_arrow(drawed_image, finish_point, length, angle, cv::Scalar(255, 255, 255), 3);

    cv::imshow("drawed_image", drawed_image);
    cv::waitKey(0);
}
```

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