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

本記事でできること

マーカを認識する例

想定シーン

ロボットをビジュアルフィードバック制御する

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

マーカ認識をする

画像認識とは別に、2次元マーカを用いて、ロボットに位置の情報や物体の状態を教えたいと思うことがあります。OpenCVではARuCoマーカという2次元マーカを使用することができます。注意点として、なんとなくQRコードのように見えますが、QRコードではありません。なので、スマホなどのアプリでは見えません。
本記事ではARuCoマーカとカメラを用いて位置を認識するプログラムを掲載します。

実行環境

• OS：Ubuntu 22.04 LTS
• 言語：C++
• クルーボ：v5.1.0

クルーボについて

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

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

プログラム抜粋

std::optional<MarkerPose> detectMarker(
        const cv::Mat& camera_image,
        const cv::Ptr<cv::aruco::Dictionary> marker_dictionary,
        const int target_marker_id = 0) {
    std::vector<int> detected_ids;
    std::vector<std::vector<cv::Point2f>> detected_boxes;

    // マーカの検出
    cv::aruco::detectMarkers(camera_image, marker_dictionary, detected_boxes, detected_ids);

    bool target_id_is_found = false;
    std::vector<cv::Point2f> box_corners;
    for (size_t i = 0; i < detected_ids.size(); ++i) {
        int detected_id = detected_ids[i];
        if (detected_id == target_marker_id) {
            target_id_is_found = true;
            box_corners = detected_boxes[i];
            break;
        }
    }

    // 目標のマーカが検出されなかった場合は、検出無しとして std::nullopt を返す。
    if (target_id_is_found == false) {
        return std::nullopt;
    }

    // マーカ位置のを求める。ここでは、マーカの中心をマーカ位置としている。
    cv::Point2f box_centroid = (box_corners[0] + box_corners[1] + box_corners[2] + box_corners[3]) / 4.f;

    // マーカの回転角度を求める。ここでは、画像座標系U軸を0度として時計回り方向を正の角度としている。
    cv::Point2f top_left_to_top_right = box_corners[1] - box_corners[0];
    const float rz_radians = std::atan2(top_left_to_top_right.y, top_left_to_top_right.x);
    const crewbo::Degrees rz = crewbo::Degrees::fromRadFloat_(rz_radians);
    return MarkerPose{box_centroid.x, box_centroid.y, rz};
}

全体プログラム

#include <iostream>
#include <optional>
#include <string>

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

#include "crewbo/crewbo.h"

struct MarkerPose {
    float u;
    float v;
    crewbo::Degrees rz;
};

std::optional<MarkerPose> detectMarker(
        const cv::Mat& camera_image,
        const cv::Ptr<cv::aruco::Dictionary> marker_dictionary,
        const int target_marker_id = 0) {
    std::vector<int> detected_ids;
    std::vector<std::vector<cv::Point2f>> detected_boxes;

    // マーカの検出
    cv::aruco::detectMarkers(camera_image, marker_dictionary, detected_boxes, detected_ids);

    bool target_id_is_found = false;
    std::vector<cv::Point2f> box_corners;
    for (size_t i = 0; i < detected_ids.size(); ++i) {
        int detected_id = detected_ids[i];
        if (detected_id == target_marker_id) {
            target_id_is_found = true;
            box_corners = detected_boxes[i];
            break;
        }
    }

    // 目標のマーカが検出されなかった場合は、検出無しとして std::nullopt を返す。
    if (target_id_is_found == false) {
        return std::nullopt;
    }

    // マーカ位置のを求める。ここでは、マーカの中心をマーカ位置としている。
    cv::Point2f box_centroid = (box_corners[0] + box_corners[1] + box_corners[2] + box_corners[3]) / 4.f;

    // マーカの回転角度を求める。ここでは、画像座標系U軸を0度として時計回り方向を正の角度としている。
    cv::Point2f top_left_to_top_right = box_corners[1] - box_corners[0];
    const float rz_radians = std::atan2(top_left_to_top_right.y, top_left_to_top_right.x);
    const crewbo::Degrees rz = crewbo::Degrees::fromRadFloat_(rz_radians);
    return MarkerPose{box_centroid.x, box_centroid.y, rz};
}

int main() {
    // カメラオブジェクトを生成する。
    const int image_width = 640;
    const int image_height = 480;
    crewbo::camera::UsbCamera camera(0, image_width, image_height);

    // マーカ辞書を読み込む。
    const cv::Ptr<cv::aruco::Dictionary> marker_dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);

    // 画像確認用のウィンドウを作成する。
    const std::string window_name = "image";
    cv::namedWindow(window_name);

    // キーボードのqキーを押下すれば終了できる。
    std::cout << "q キー押下で終了します。 " << std::endl;
    while (true) {
        const cv::Mat camera_image = camera.fetchSingleFrame_();

        // マーカの位置姿勢を検出する。
        const std::optional<MarkerPose> maybe_marker_pose = detectMarker(camera_image, marker_dictionary);

        // 検出できなかった場合は、ロボットに指令せずにループ先頭に戻る。
        if (maybe_marker_pose == std::nullopt) {
            cv::imshow(window_name, camera_image);
            if (cv::waitKey(1) == 'q') {
                break;
            };
            continue;
        }

        // 検出できた場合。
        const MarkerPose marker_pose = maybe_marker_pose.value();

        cv::circle(
                camera_image, {static_cast<int>(marker_pose.u), static_cast<int>(marker_pose.v)}, 5, {0, 0, 255}, -1);
        cv::imshow(window_name, camera_image);
        if (cv::waitKey(1) == 'q') {
            break;
        };
    }
}

おわりに

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