環境

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

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

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

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

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

ソースコード

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は以下です

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>

ビルド

cd ~/catkin_ws
catkin build

実行

roslaunch cam_lecture sim_coke_detect.launch

HSVで色検出した結果

膨張収縮処理をした結果

輪郭検出した結果

領域を切り出した結果(rectangleを使用)

領域を切り出した結果(minAreaRectを使用)

参考

ロボットビジョンの準備
 pythonで赤い物体を認識しよう
 色空間の変換
 収縮・膨張フィルタ
 OpenCVで図形を書く

目次ページへのリンク

ROS講座の目次へのリンク