デプスセンサ・３Dカメラでワークの頂点を認識する【道具としての OpenCV 画像認識】

# 本記事でできること
カラー画像
![image (5).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/37698/16a88594-e4e3-4e4b-1de6-6ce1a517c0e1.png)


デプスセンサからもっとも近い箇所に☓（バツ印）を描画した深度画像
![image (2).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/37698/0e4761f5-80d6-0f6f-d613-7b3983940ed8.png)


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

## デプスセンサ・３Dカメラでワークの頂点を認識する
ビジュアルフィードバック制御でロボットを自動化して、ばら積みワークの認識とピッキングなどを実装したい場合があります。ばら積みでは、たとえば、ばりが多く付いた鋳物部品や、射出成形された樹脂部品、不定形物体の食品部材などがよくあります。本記事では、ばら積みされた物体について、デプスセンサ・３Dカメラでワークの頂点を認識することで、どこにロボットを移動させればよいかを計算する画像認識プログラムを掲載します。ロボットの制御プログラムは別途掲載予定です。

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

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

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

## プログラム抜粋
```c++
cv::Point3f detect_mountaintop(const cv::Mat& depth_image) {
    cv::Mat result_depth_image = depth_image.clone();
    const float min_value_limit = 5.f;  // [mm]
    float min_value = 5000;
    cv::Point min_point(0.f, 0.f);

    for (int i = 0; i < depth_image.cols; i++) {
        for (int j = 0; j < depth_image.rows; j++) {
            const double distance = static_cast<double>(depth_image.at<float>(cv::Point(i, j)));
            if (distance > min_value_limit) {
                if (distance < min_value) {
                    min_value = static_cast<float>(distance);
                    min_point = cv::Point(i, j);
                }
            }
        }
    }

    return cv::Point3f(static_cast<float>(min_point.x), static_cast<float>(min_point.y), min_value);
}
```

## 全体プログラム
```c++
#include "crewbo/crewbo.h"
#include "project_lib/vision_assistant.h"

#include <fmt/format.h>
#include <iostream>

cv::Point3f detect_mountaintop(const cv::Mat& depth_image) {
    cv::Mat result_depth_image = depth_image.clone();
    const float min_value_limit = 5.f;  // [mm]
    float min_value = 5000;
    cv::Point min_point(0.f, 0.f);

    for (int i = 0; i < depth_image.cols; i++) {
        for (int j = 0; j < depth_image.rows; j++) {
            const double distance = static_cast<double>(depth_image.at<float>(cv::Point(i, j)));
            if (distance > min_value_limit) {
                if (distance < min_value) {
                    min_value = static_cast<float>(distance);
                    min_point = cv::Point(i, j);
                }
            }
        }
    }

    return cv::Point3f(static_cast<float>(min_point.x), static_cast<float>(min_point.y), min_value);
}

int main(void) {
    crewbo::camera::RealSense camera;

    while (cv::waitKey(1) != 'q') {
        const cv::Mat depth_image = camera.fetchDepthFrame_();
        const cv::Point3f mountaintop = detect_mountaintop(depth_image);
        fmt::print("mountaintop_depth_value [mm] = {:4.2f}\n", mountaintop.z);
        const cv::Mat colorized_image = crewbo::camera::RealSense::colorizeDepthImage_(depth_image);
        const cv::Mat result_image = va::draw_cross(
                colorized_image, cv::Point(static_cast<int>(mountaintop.x), static_cast<int>(mountaintop.y)));
        cv::imshow("RealSense depth image", result_image);
    }
}
```

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