このページでは、OpenCVの画像処理により図形を検出して移動ロボット (Raspberry Piマウス) を追従させるRTCの作成手順を説明します。
HoughCirclesはハフ変換を用いてグレースケール画像から円を検出する関数です。 詳細は以下のページを参照。
カメラで取得した画像をグレースケール画像に変換後、HoughCircles関数で円を検出します。 検出した円の方向に移動ロボットが回転するように制御します。 具体的には、検出した円の位置が画像の右側の場合は右回り、左側の場合は左回りの回転する目標速度を指令します。 また、動作確認用に円の位置情報を付加した画像を出力します。
以降はRTCの基本的な作成方法を理解している前提で進めます。 基本的な作成手順は以下のページを参照。
RTCBuilderで、以下の仕様のRTCのひな型コードを生成します。
コンポーネント名称 | CircleTracking |
アクティビティ | onActivated、onDeactivated、onExecute |
言語 | C++ |
InPort | |
ポート名 | image_in |
型 | RTC::CameraImage |
説明 | 入力画像 |
InPort | |
ポート名 | velocity_in |
型 | RTC::TimedVelocity2D |
説明 | 変更前の目標速度 |
OutPort | |
ポート名 | image_out |
型 | RTC::CameraImage |
説明 | 円の情報を付加した画像 |
OutPort | |
ポート名 | velocity_out |
型 | RTC::TimedVelocity2D |
説明 | 変更後の目標速度 |
Configuration | |
パラメーター名 | speed_r |
型 | double |
デフォルト値 | 0.5 |
制約 | 0.0<x<2.0 |
Widget | slider |
Step | 0.01 |
説明 | 図形の位置により、右回転、左回転する場合の回転速度 |
Configuration | |
パラメーター名 | houghcircles_dp |
型 | double |
デフォルト値 | 2 |
Widget | text |
説明 | HoughCircles関数の引数dp |
Configuration | |
パラメーター名 | houghcircles_minDist |
型 | double |
デフォルト値 | 30 |
Widget | text |
説明 | HoughCircles関数の引数minDist |
Configuration | |
パラメーター名 | houghcircles_param1 |
型 | double |
デフォルト値 | 100 |
Widget | text |
説明 | HoughCircles関数の引数param1 |
Configuration | |
パラメーター名 | houghcircles_param2 |
型 | double |
デフォルト値 | 100 |
Widget | text |
説明 | HoughCircles関数の引数param2 |
Configuration | |
パラメーター名 | houghcircles_minRadius |
型 | double |
デフォルト値 | 0 |
Widget | text |
説明 | HoughCircles関数の引数minRadius |
Configuration | |
パラメーター名 | houghcircles_maxRadius |
型 | double |
デフォルト値 | 0 |
Widget | text |
説明 | HoughCircles関数の引数maxRadius |
RTC::CameraImage型は画像データを格納するデータ型です。
struct CameraImage { /// Time stamp. Time tm; /// Image pixel width. unsigned short width; /// Image pixel height. unsigned short height; /// Bits per pixel. unsigned short bpp; /// Image format (e.g. bitmap, jpeg, etc.). string format; /// Scale factor for images, such as disparity maps, where the integer pixel value should be divided by this factor to get the real pixel value. double fDiv; /// Raw pixel data. sequence<octet> pixels; };
このデータ型には、画像の幅width、画像の高さheight、画像データpixels等を設定できます。
以下のファイルを編集します。
srcフォルダのCMakeLists.txtをメモ帳などで開いて編集してください。 ここでは、OpenCVを利用するための設定を行います。
以下のようにfind_packageによりOpenCVのライブラリを検出します。 find_packageの行を追加してください。
set(comp_srcs CircleTracking.cpp ) set(standalone_srcs CircleTrackingComp.cpp) find_package(OpenCV REQUIRED) #追加
次にリンクするライブラリに、OpenCVのライブラリを追加します。 以下の2か所を変更します。
# target_link_libraries(${PROJECT_NAME} ${OPENRTM_LIBRARIES}) #修正前 target_link_libraries(${PROJECT_NAME} ${OPENRTM_LIBRARIES} ${OpenCV_LIBS}) #修正後、${OpenCV_LIBS}を追加する
# target_link_libraries(${PROJECT_NAME}Comp ${OPENRTM_LIBRARIES} ${OpenCV_LIBS}) #修正前 target_link_libraries(${PROJECT_NAME}Comp ${OPENRTM_LIBRARIES} ${OpenCV_LIBS}) #修正後、${OpenCV_LIBS}を追加する
include/CircleTracking/CircleTracking.hの編集を行います。 まず、37行目付近でインクルードファイルを記述します。
#include <rtm/DataInPort.h> #include <rtm/DataOutPort.h> #include <opencv2/opencv.hpp> //追加
314行目付近にprivate:という記述があるため、その下にm_imageBuff、m_outputBuff、m_directionの3つのメンバ変数を追加します。
private: cv::Mat m_imageBuff; //追加、入力画像を格納する変数 cv::Mat m_outputBuff; //追加、円の情報を付加した画像を格納する変数 int m_direction; //追加、移動ロボットの回転方向を格納する変数
src/CircleTracking.cppの編集を行います。 onActivated、onDeactivated、onExecuteの3つの関数を編集します。
RTC::ReturnCode_t CircleTracking::onActivated(RTC::UniqueId /*ec_id*/) { // OutPortの画面サイズを0に設定 m_image_out.width = 0; m_image_out.height = 0; //進行方向を0(回転方向を指定しない)に設定 m_direction = 0; return RTC::RTC_OK; }
RTC::ReturnCode_t CircleTracking::onDeactivated(RTC::UniqueId /*ec_id*/) { if (!m_outputBuff.empty()) { // 画像用メモリの解放 m_imageBuff.release(); m_outputBuff.release(); } return RTC::RTC_OK; }
RTC::ReturnCode_t CircleTracking::onExecute(RTC::UniqueId /*ec_id*/) { if (m_image_inIn.isNew()) { cv::Mat gray; std::vector<cv::Vec3f> circles; // 画像データの読み込み m_image_inIn.read(); // InPortとOutPortの画面サイズ処理およびイメージ用メモリの確保 if (m_image_in.width != m_image_out.width || m_image_in.height != m_image_out.height) { m_image_out.width = m_image_in.width; m_image_out.height = m_image_in.height; m_imageBuff.create(cv::Size(m_image_in.width, m_image_in.height), CV_8UC3); m_outputBuff.create(cv::Size(m_image_in.width, m_image_in.height), CV_8UC3); } // InPortの画像データをm_imageBuffにコピー std::memcpy(m_imageBuff.data, (void*)&(m_image_in.pixels[0]), m_image_in.pixels.length()); //カラー画像をグレースケールに変換 cv::cvtColor(m_imageBuff, gray, cv::COLOR_BGR2GRAY); //HoughCircles関数で円を検出する cv::HoughCircles(gray, circles, cv::HOUGH_GRADIENT, m_houghcircles_dp, m_houghcircles_minDist, m_houghcircles_param1, m_houghcircles_param2, m_houghcircles_minRadius, m_houghcircles_maxRadius); //円を検出できた場合の処理 if (!circles.empty()) { //円の位置が画像の左側の場合は左回りに回転するように設定 if (circles[0][0] < gray.cols / 2) { m_direction = 1; } //円の位置が画像の右側の場合は右回りに回転するように設定 else { m_direction = 2; } } //円を検出できなかった場合は回転方向の指定をしないように設定 else { m_direction = 0; } //元のカラー画像をコピーして円の情報を画像に追加 std::memcpy(m_outputBuff.data, (void*)&(m_image_in.pixels[0]), m_image_in.pixels.length()); for (auto circle : circles) { cv::circle(m_outputBuff, cv::Point(static_cast<int>(circle[0]), static_cast<int>(circle[1])), static_cast<int>(circle[2]), cv::Scalar(0, 0, 255), 2); } // 画像データのサイズ取得 int len = m_outputBuff.channels() * m_outputBuff.cols * m_outputBuff.rows; m_image_out.pixels.length(len); // 円の情報を付加した画像データをOutPortにコピー std::memcpy((void*)&(m_image_out.pixels[0]), m_outputBuff.data, len); //画像データを出力 m_image_outOut.write(); } if (m_velocity_inIn.isNew()) { //速度指令値を読み込み m_velocity_inIn.read(); m_velocity_out = m_velocity_in; //円が画像の左側にある場合、左回りに回転する if (m_direction == 1) { m_velocity_out.data.va = m_speed_r; } //円が画像の右側にある場合、右回りに回転する else if (m_direction == 2) { m_velocity_out.data.va = -m_speed_r; } //速度指令値を出力 m_velocity_outOut.write(); } return RTC::RTC_OK; }
ここからはRTSystemEditorで作業します。 Raspberry Piマウスを使用する場合は、Raspberry Piのアクセスポイントに接続した状態で作業してください。 以下のページのRobotControllerコンポーネントが必要なため、実機での動作確認まで進めておいてください。
動作確認には、Raspberry Piマウス、USBカメラ、カメラ用マウント、LiDAR付属のネジ、円形状の図形を印刷した紙が必要です。 講習会ではUSBカメラとカメラ用マウントは接続済みです。紙も配布しています。
専用LiDARマウントかマルチLiDARマウントかで使用するネジが異なります。
まず、以下の専用LiDARマウントの場合は、ネジは2個付属しているので、それを使用してください。
以下のマルチLiDARマウントの場合は、なべタッピングネジ3-8を使用してください。
以下のようにRaspberry Piマウス前方の2か所で固定します。
PCとUSBカメラをUSBポートで接続してください。
動作確認には、以下の5つのRTCの起動が必要です。
RaspberryPiMouseRTCとRobotControllerコンポーネントの起動ついては、以下のページの手順を参考にしてください。
OpenCVCamera、CameraViewerはOpenRTM-aist付属のサンプルコンポーネントです。 Windows 10の場合は、画面左下の「ここに入力して検索」にC++_OpenCV-Examplesと入力して、C++_OpenCV-Examplesを選択したら起動するエクスプローラからCameraViewer.batとOpenCVCamera.batをダブルクリックして実行してください。
Ubuntuの場合はビルドとインストール作業が必要です。
CircleTrackingはビルドで生成したCircleTrackingComp.exeを実行してください。
RTSystemEditor上で以下のようにポートを接続してください。
RTC名 | OutPort名 | RTC名 | InPort名 |
OpenCVCamera0 | out | CircleTracking0 | image_in |
RobotController0 | out | CircleTracking0 | velocity_in |
CircleTracking0 | image_out | CameraViewer0 | in |
CircleTracking0 | velocity_out | RaspberryPiMouseRTC0 | target_velocity_in |
RaspberryPiMouseRTC0 | ir_sensor_out | RobotController | in |
RTCをアクティブ化すれば動作確認を開始します。
カメラの前で円形状の図形を印刷した紙を左右に動かして、動作を確認してください。
OpenCVCameraコンポーネントがRaspberry Piマウスに取り付けたUSBカメラではなく、別のUSBカメラやノートPC内蔵カメラを使用する場合があります。 この場合は他のカメラの画像が表示されているので、RTSystemEditorでOpenCVCamera0を選択して、コンフィギュレーションパラメータを編集します。
以下のdevice_numを変更して確認してください。
また、円の誤検出が多い場合、RTSystemEditorでCircleTracking0を選択して、コンフィギュレーションパラメータを変更して試してみてください。
HoughCircles関数の引数の詳細についてはOpenCVのドキュメントを参考にしてください。