ROS講座79 画像処理でコーラ缶を検知する

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |
| Gazebo | 11.9.0 |
| OpenCV | 4.2 |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
画像処理を使ってコーラ缶を検出します。CNNなどの深層学習を使うモダンな方法ではなく古典的な手法を用います。簡単に流れを説明すると

* 画像をHSVに直して色のフィルタをかける。
* 膨張・収縮処理を用いてノイズを除去する。
* 外輪郭を取得する。
* 外輪郭を覆う四角形の領域を取得

# ソースコード

```c++:cam_lecture/src/cam_color_detector.cpp
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <vision_msgs/Detection2DArray.h>

class ColorDetector{

public:
  ColorDetector()
    : nh_(""), pnh_("~"), it_(nh_)
  {
    camera_sub_ = it_.subscribeCamera("/head_camera/image_raw", 1, &ColorDetector::imageCallback, this);
    detect_pub_ = nh_.advertise<vision_msgs::Detection2DArray>("detection", 1);
  }

  void imageCallback(const sensor_msgs::ImageConstPtr& image_msg,
               const sensor_msgs::CameraInfoConstPtr& info_msg)
  {
    // get paramater
    std::string debug_output = "";
    pnh_.getParamCached("debug_output", debug_output);

    int h_min = 0;
    pnh_.getParamCached("h_min", h_min);
    int h_max = 180;
    pnh_.getParamCached("h_max", h_max);
    int s_min = 0;
    pnh_.getParamCached("s_min", s_min);
    int s_max = 255;
    pnh_.getParamCached("s_max", s_max);
    int v_min = 0;
    pnh_.getParamCached("v_min", v_min);
    int v_max = 255;
    pnh_.getParamCached("v_max", v_max);

    cv::Mat original_image;
    cv_bridge::CvImagePtr input_bridge;
    try {
      input_bridge = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
      original_image = input_bridge->image;
    }
    catch (cv_bridge::Exception& ex){
      ROS_ERROR("[draw_frames] Failed to convert image");
      return;
    }
    if (debug_output == "input") {
      cv::imshow("Debug", original_image);  
      cv::waitKey(1);  
    }

    // bluer
    cv::Mat blur_image;
    cv::Size ksize = cv::Size(3, 3);
    cv::GaussianBlur(original_image, blur_image, ksize, 0);
    if (debug_output == "blue") {
      cv::imshow("Debug", blur_image);
      cv::waitKey(1);  
    }

    // convert RBG -> HSV
    cv::Mat hsv_image;
    cv::cvtColor(blur_image, hsv_image, CV_BGR2HSV);

    // hsv range filter
    cv::Mat hsv_range_image;
    cv::Scalar min_range = cv::Scalar(h_min, s_min, v_min);
    cv::Scalar max_range = cv::Scalar(h_max, s_max, v_max);
    cv::inRange(hsv_image, min_range, max_range, hsv_range_image);
    if (debug_output == "range") {
      cv::Mat masked_image;
      original_image.copyTo(masked_image, hsv_range_image);
      cv::imshow("Debug", masked_image);
      cv::waitKey(1);  
    }

    // opening
    cv::Mat mid_mask;
    cv::Mat opened_mask;
    cv::dilate(hsv_range_image, mid_mask, cv::Mat(), cv::Point(-1,-1), 2);
    cv::erode(mid_mask, opened_mask, cv::Mat(), cv::Point(-1,-1), 2);
    if (debug_output == "opening") {
      cv::Mat masked_image;
      original_image.copyTo(masked_image, opened_mask);
      cv::imshow("Debug", masked_image);
      cv::waitKey(1);  
    }

    // get contours
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(opened_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
    if (debug_output == "contour") {
      cv::Mat overlay_image;
      original_image.copyTo(overlay_image);
      for(auto c: contours){
        cv::polylines(overlay_image, c, true, cv::Scalar(255, 0, 0), 2, cv::LINE_AA);
      }
      cv::imshow("Debug", overlay_image);
      cv::waitKey(1);  
    }

    // get convex
    std::vector<std::vector<cv::Point>> approxes;
    for(auto c: contours){
      float area = cv::contourArea(c);
      if(area < 100){
        continue;
      }
      std::vector<cv::Point> approx;
      cv::convexHull(c, approx);
      approxes.push_back(approx);
    }
    if (debug_output == "convex") {
      cv::Mat overlay_image;
      original_image.copyTo(overlay_image);
      for(auto a: approxes){
        cv::polylines(overlay_image, a, true, cv::Scalar(255, 0, 0), 2, cv::LINE_AA);
      }
      cv::imshow("Debug", overlay_image);
      cv::waitKey(1);  
    }

    // get boounding box
    std::vector<cv::Rect> bboxes;
    for(auto a: approxes){
      cv::Rect rect = cv::boundingRect(a);
      bboxes.push_back(rect);
    }
    if (debug_output == "bbox") {
      cv::Mat overlay_image;
      original_image.copyTo(overlay_image);
      for(auto r: bboxes){
        cv::rectangle(overlay_image, r.tl(), r.br(), cv::Scalar(255, 0, 0), 2, cv::LINE_AA);
      }
      cv::imshow("Debug", overlay_image);
      cv::waitKey(1);  
    }

    // rotation rect
    std::vector<cv::RotatedRect> rects;
    for(auto a: approxes){
      cv::RotatedRect rect = cv::minAreaRect(a);
      rects.push_back(rect);
    }
    if (debug_output == "rect") {
      cv::Mat overlay_image;
      original_image.copyTo(overlay_image);
      for(auto r: rects){
        std::vector<cv::Point2f> points(4);
        r.points(points.data());
        std::vector<cv::Point> draw_points;
        for(auto p : points){
          draw_points.push_back(p);
        }
        cv::polylines(overlay_image, draw_points, true, cv::Scalar(255, 0, 0), 2, cv::LINE_AA);
      }
      cv::imshow("Debug", overlay_image);
      cv::waitKey(1);  
    }

    // result
    vision_msgs::Detection2DArray output;
    output.header = image_msg->header;
    for(auto r: bboxes){
      vision_msgs::Detection2D detect;
      detect.bbox.center.x = r.x + r.width / 2;
      detect.bbox.center.y = r.y + r.height / 2;
      detect.bbox.size_x = r.width;
      detect.bbox.size_y = r.height;
      output.detections.push_back(detect);
    }
    detect_pub_.publish(output);
  }

  ros::NodeHandle nh_;
  ros::NodeHandle pnh_;
  ros::Publisher detect_pub_;
  image_transport::ImageTransport it_;
  image_transport::CameraSubscriber camera_sub_;
};

int main(int argc, char** argv){
  ros::init(argc, argv, "cam_color_detector");
  ColorDetector detector;
  ros::spin();
}
```

以下細かいところを解説していきます。

* RGB→HSV変換
`cvtColor`関数でRGB表記をHSV表記に変換します。H:色相（何色っぽいか）、S:彩度(くすんでいるか、鮮やかか)、V:明度(明るいか)の基準のほうが色として指定しやすいのでよく使われます。
* 特定の範囲の色の切り抜き
`inRange`関数でHSV表記で一定の領域の色を抽出して、白黒2値（正確にはグレースケールで0と255のみの画像）のマスク画像を生成します。
* ノイズ除去
`dilate`では、`erode`関数では膨張、収縮を行います。これによって赤い缶の中で検出されていない白い文字の部分をつぶします。
* 輪郭検出
`findContours`関数では3つの各缶の外輪郭の点を検出します。`convexHull`は先ほどの点を形が凸になるようにフィルターをかけます。
* 領域検出
領域検出は2通りの方法があります。`rectangle`では各辺が垂直水平になるような長方形で囲みます。`minAreaRect`ではな斜めになるもの含めて最小の面積になるような長方形を生成します。

起動launchは以下です

```xml:cam_lecture/launch/coke_detect.launch
<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="model" default="$(find cam_lecture)/urdf/camera_sim.urdf" />
  <arg name="rvizconfig" default="$(find cam_lecture)/rviz/camera_sim.rviz" />
  <param name="robot_description" type="str" textfile="$(arg model)"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="sensor_station_coke.world" />
  </include>

  <node name="cam_color_detect" pkg="cam_lecture" type="cam_color_detector" output="screen">
    <param name="debug_output" value="rect"/>
    <param name="h_min" value="160"/>
    <param name="h_max" value="180"/>
    <param name="s_min" value="0"/>
    <param name="s_max" value="255"/>
    <param name="v_min" value="0"/>
    <param name="v_max" value="255"/>
  </node>

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>
```

# ビルド
```shell
cd ~/catkin_ws
catkin build
```

# 実行

```shell
roslaunch cam_lecture sim_coke_detect.launch 
```

HSVで色検出した結果
![cam_coke1.png](https://qiita-image-store.s3.amazonaws.com/0/254442/ece44ed2-0434-c94c-33d3-298f02a8dcba.png)

膨張収縮処理をした結果
![cam_coke2.png](https://qiita-image-store.s3.amazonaws.com/0/254442/2439a273-e3f2-fe8a-99fe-7a6ee3847425.png)

輪郭検出した結果
![cam_coke3.png](https://qiita-image-store.s3.amazonaws.com/0/254442/679685e5-3907-7021-1a93-9189e5d49352.png)

領域を切り出した結果(rectangleを使用)
![bbox.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/6d0edd30-9a07-baa1-1095-69fce8b956bc.gif)

領域を切り出した結果(minAreaRectを使用)
![rect.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/ceb91563-de13-92c0-5020-c17c4145451a.gif)

# 参考
[ロボットビジョンの準備](http://ishi.main.jp/ros/ros_opencv_brg.html)
[pythonで赤い物体を認識しよう](https://qiita.com/odaman68000/items/ae28cf7bdaf4fa13a65b)
[色空間の変換](http://labs.eecs.tottori-u.ac.jp/sd/Member/oyamada/OpenCV/html/py_tutorials/py_imgproc/py_colorspaces/py_colorspaces.html)
[収縮・膨張フィルタ](http://labs.eecs.tottori-u.ac.jp/sd/Member/oyamada/OpenCV/html/py_tutorials/py_imgproc/py_morphological_ops/py_morphological_ops.html)
[OpenCVで図形を書く](https://note.nkmk.me/python-opencv-draw-function/)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)