OpenCVのライブラリを用いて、物体追跡を行うコンポーネントです。
InPortからの画像データを表示し、マウスで選択されたオブジェクトを追跡するコンポーネント。
OutPortからは、物体追跡画像と、マウスで選択した位置からの移動量を出力する。
画像のサイズと、明度、彩度はコンフィグレーションにて変更可能。
作成するRTCの仕様は以下のとおりです。
// -*- C++ -*- /*! * @file ObjectTracking.cpp * @brief Object tracking component * @date $Date$ * * $Id$ */ #include "ObjectTracking.h" #define HIST_RANGE_MAX 180.0 #define HIST_RANGE_MIN 0.0 // 入力画像用IplImage IplImage* g_image; // CamShiftトラッキング用変数 CvPoint g_origin; CvRect g_selection; // // 初期追跡領域の設定判別フラグ値 (0: 設定なし, 1: 設定あり) int g_select_object; // トラッキングの開始/停止用フラグ値 (0: 停止, -1: 開始, 1: トラッキング中) int g_track_object; // オブジェクト選択判定用フラグ(0: 1以外 1: 選択された直後) int g_selected_flag; // 選択された範囲の中心座標 float g_selected_x; float g_selected_y; // Module specification static const char* objecttracking_spec[] = { "implementation_id", "ObjectTracking", "type_name", "ObjectTracking", "description", "Object tracking component", "version", "1.0.0", "vendor", "AIST", "category", "Category", "activity_type", "PERIODIC", "kind", "DataFlowComponent", "max_instance", "1", "language", "C++", "lang_type", "compile", "exec_cxt.periodic.rate", "1.0", // Configuration variables "conf.default.brightness_max", "256", "conf.default.brightness_min", "10", "conf.default.saturation_min", "30", "conf.default.image_height", "240", "conf.default.image_width", "320", "" }; /*! * @brief constructor * @param manager Maneger Object */ ObjectTracking::ObjectTracking(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager), m_in_imageIn("in_image", m_in_image), m_tracking_imageOut("tracing_image", m_tracking_image), m_displacementOut("displacement", m_displacement), dummy(0), m_hsv(0), m_hue(0), m_mask(0), m_backproject(0), m_hist(0), m_backproject_mode(0),m_hdims(16), m_init_flag(0) { // Registration: InPort/OutPort/Service // Set InPort buffers registerInPort("in_image", m_in_imageIn); // Set OutPort buffer registerOutPort("tracing_image", m_tracking_imageOut); registerOutPort("displacement", m_displacementOut); m_hranges_arr[0] = HIST_RANGE_MIN; m_hranges_arr[1] = HIST_RANGE_MAX; m_hranges = m_hranges_arr; } /*! * @brief destructor */ ObjectTracking::~ObjectTracking() { } RTC::ReturnCode_t ObjectTracking::onInitialize() { // Bind variables and configuration variable bindParameter("brightness_max", m_b_max, "256"); bindParameter("brightness_min", m_b_min, "10"); bindParameter("saturation_min", m_s_min, "30"); bindParameter("image_height", m_img_height, "240"); bindParameter("image_width", m_img_width, "320"); m_displacement.data.length(2); return RTC::RTC_OK; } RTC::ReturnCode_t ObjectTracking::onActivated(RTC::UniqueId ec_id) { m_init_flag = 0; // ウインドウを生成する cvNamedWindow( "ObjectTracking", 1 ); // マウス操作時のコールバック処理の登録 cvSetMouseCallback( "ObjectTracking", on_mouse, 0 ); g_selected_flag = 0; g_selected_x = 0.0;; g_selected_y = 0.0;; return RTC::RTC_OK; } RTC::ReturnCode_t ObjectTracking::onDeactivated(RTC::UniqueId ec_id) { if (m_init_flag) { // メモリを解放する cvReleaseImage(&g_image); cvReleaseImage(&m_hsv); cvReleaseImage(&m_hue); cvReleaseImage(&m_mask); cvReleaseImage(&m_backproject); } // ウインドウを破棄する cvDestroyWindow("ObjectTracking"); m_init_flag = 0; return RTC::RTC_OK; } RTC::ReturnCode_t ObjectTracking::onExecute(RTC::UniqueId ec_id) { if (m_in_imageIn.isNew()) { m_in_imageIn.read(); if(!m_init_flag) allocateBuffers(); memcpy(g_image->imageData,(void *)&(m_in_image.data[0]),m_in_image.data.length()); cvShowImage("ObjectTracking", g_image); // キャプチャされた画像をHSV表色系に変換してhsvに格納 cvCvtColor( g_image, m_hsv, CV_BGR2HSV ); // g_track_objectフラグが0以下なら、以下の処理を行う if( g_track_object ) { cvInRangeS( m_hsv, cvScalar(HIST_RANGE_MIN,m_s_min,MIN(m_b_min,m_b_max),0), cvScalar(HIST_RANGE_MAX,256,MAX(m_b_min,m_b_max),0), m_mask ); cvSplit( m_hsv, m_hue, 0, 0, 0 ); if( g_track_object < 0 ) calcHistogram(); // バックプロジェクションを計算する cvCalcBackProject( &m_hue, m_backproject, m_hist ); // backProjectionのうち、マスクが1であるとされた部分のみ残す cvAnd( m_backproject, m_mask, m_backproject, 0 ); // CamShift法による領域追跡を実行する cvCamShift( m_backproject, m_track_window, cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ), &m_track_comp, &m_track_box ); m_track_window = m_track_comp.rect; if( m_backproject_mode ) cvCvtColor( m_backproject, g_image, CV_GRAY2BGR ); if( g_image->origin ) m_track_box.angle = -m_track_box.angle; cvEllipseBox( g_image, m_track_box, CV_RGB(255,0,0), 3, CV_AA, 0 ); // マウスで選択された領域の中心座標を保存 if (g_selected_flag) { g_selected_x = m_track_box.center.x; g_selected_y = m_track_box.center.y; g_selected_flag = 0; } // マウスで選択された位置からの移動量をOutPortから出力 m_displacement.data[0] = m_track_box.center.x - g_selected_x; m_displacement.data[1] = -(m_track_box.center.y - g_selected_y); m_displacementOut.write(); } // マウスで選択中の初期追跡領域の色を反転させる if( g_select_object && g_selection.width > 0 && g_selection.height > 0 ) { cvSetImageROI( g_image, g_selection ); cvXorS( g_image, cvScalarAll(255), g_image, 0 ); cvResetImageROI( g_image ); } // 画像を表示する cvShowImage( "ObjectTracking", g_image ); // 画像をOutPortから出力する int len = g_image->nChannels * g_image->width * g_image->height; m_tracking_image.data.length(len); memcpy((void *)&(m_tracking_image.data[0]),g_image->imageData,len); m_tracking_imageOut.write(); // キー入力を待ち、押されたキーによって処理を分岐させる int c = cvWaitKey(10); // while無限ループから脱出(プログラムを終了) if( (char) c == 27 ) { this->exit(); } } return RTC::RTC_OK; } /*! * 全てのイメージ用メモリの確保 */ void ObjectTracking::allocateBuffers() { g_image = cvCreateImage( cvSize(m_img_width,m_img_height),8, 3 ); m_hsv = cvCreateImage( cvSize(m_img_width,m_img_height),8, 3 ); m_hue = cvCreateImage( cvSize(m_img_width,m_img_height),8, 1 ); m_mask = cvCreateImage( cvSize(m_img_width,m_img_height),8, 1 ); m_backproject = cvCreateImage( cvSize(m_img_width,m_img_height),8, 1 ); m_hist = cvCreateHist( 1, &m_hdims, CV_HIST_ARRAY, &m_hranges, 1 ); m_init_flag = 1; } /*! * ヒストグラムの計算 */ void ObjectTracking::calcHistogram() { float max_val = 0.f; cvSetImageROI( m_hue, g_selection ); cvSetImageROI( m_mask, g_selection ); // ヒストグラムを計算し、最大値を求める cvCalcHist( &m_hue, m_hist, 0, m_mask ); cvGetMinMaxHistValue( m_hist, 0, &max_val, 0, 0 ); // ヒストグラムの縦軸(頻度)を0-255のダイナミックレンジに正規化 cvConvertScale( m_hist->bins, m_hist->bins, max_val ? 255. / max_val : 0., 0 ); // hue,mask画像に設定されたROIをリセット cvResetImageROI( m_hue ); cvResetImageROI( m_mask ); m_track_window = g_selection; // track_objectをトラッキング中にする g_track_object = 1; } extern "C" { void ObjectTrackingInit(RTC::Manager* manager) { RTC::Properties profile(objecttracking_spec); manager->registerFactory(profile, RTC::Create<ObjectTracking>, RTC::Delete<ObjectTracking>); } // // マウスドラッグによって初期追跡領域を指定する // // 引数: // event : マウス左ボタンの状態 // x : マウスが現在ポイントしているx座標 // y : マウスが現在ポイントしているy座標 // flags : 本プログラムでは未使用 // param : 本プログラムでは未使用 // void on_mouse( int event, int x, int y, int flags, void* param ) { // 画像が取得されていなければ、処理を行わない if( !g_image ) return; // 原点の位置に応じてyの値を反転(画像の反転ではない) if( g_image->origin ) y = g_image->height - y; // マウスの左ボタンが押されていれば以下の処理を行う if( g_select_object ) { g_selection.x = MIN(x,g_origin.x); g_selection.y = MIN(y,g_origin.y); g_selection.width = g_selection.x + CV_IABS(x - g_origin.x); g_selection.height = g_selection.y + CV_IABS(y - g_origin.y); g_selection.x = MAX( g_selection.x, 0 ); g_selection.y = MAX( g_selection.y, 0 ); g_selection.width = MIN( g_selection.width, g_image->width ); g_selection.height = MIN( g_selection.height, g_image->height ); g_selection.width -= g_selection.x; g_selection.height -= g_selection.y; } // マウスの左ボタンの状態によって処理を分岐 switch( event ) { case CV_EVENT_LBUTTONDOWN: // マウスの左ボタンが押されたのであれば、 // 原点および選択された領域を設定 g_origin = cvPoint(x,y); g_selection = cvRect(x,y,0,0); g_select_object = 1; break; case CV_EVENT_LBUTTONUP: // マウスの左ボタンが離されたとき、widthとheightがどちらも正であれば、 // g_track_objectフラグを開始フラグにする g_select_object = 0; if( g_selection.width > 0 && g_selection.height > 0 ) { g_track_object = -1; g_selected_flag = 1; } break; } } };
// -*- C++ -*- /*! * @file ObjectTracking.h * @brief Object tracking component * @date $Date$ * * $Id$ */ #ifndef OBJECTTRACKING_H #define OBJECTTRACKING_H #include <rtm/Manager.h> #include <rtm/DataFlowComponentBase.h> #include <rtm/CorbaPort.h> #include <rtm/DataInPort.h> #include <rtm/DataOutPort.h> #include <rtm/idl/BasicDataTypeSkel.h> #include <cv.h> #include <highgui.h> #include <stdio.h> #include <ctype.h> using namespace RTC; /*! * @class ObjectTracking * @brief Object tracking component * */ class ObjectTracking : public RTC::DataFlowComponentBase { public: /*! * @brief constructor * @param manager Maneger Object */ ObjectTracking(RTC::Manager* manager); /*! * @brief destructor */ ~ObjectTracking(); /*! * * The initialize action (on CREATED->ALIVE transition) * formaer rtc_init_entry() * * @return RTC::ReturnCode_t * * */ virtual RTC::ReturnCode_t onInitialize(); /*** * * The activated action (Active state entry action) * former rtc_active_entry() * * @param ec_id target ExecutionContext Id * * @return RTC::ReturnCode_t * * */ virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); /*** * * The deactivated action (Active state exit action) * former rtc_active_exit() * * @param ec_id target ExecutionContext Id * * @return RTC::ReturnCode_t * * */ virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); /*** * * The execution action that is invoked periodically * former rtc_active_do() * * @param ec_id target ExecutionContext Id * * @return RTC::ReturnCode_t * * */ virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); /*! * 入力された1つの色相値をRGBに変換する * * 引数: * hue : HSV表色系における色相値H * 戻り値: * CvScalar: RGBの色情報がBGRの順で格納されたコンテナ */ CvScalar hsv2rgb( float hue ) { int rgb[3], p, sector; static const int sector_data[][3]= {{0,2,1}, {1,2,0}, {1,0,2}, {2,0,1}, {2,1,0}, {0,1,2}}; hue *= 0.033333333333333333333333333333333f; sector = cvFloor(hue); p = cvRound(255*(hue - sector)); p ^= sector & 1 ? 255 : 0; rgb[sector_data[sector][0]] = 255; rgb[sector_data[sector][1]] = 0; rgb[sector_data[sector][2]] = p; return cvScalar(rgb[2], rgb[1], rgb[0],0); } /*! * 全てのイメージ用バッファの確保 */ void allocateBuffers(); /*! * ヒストグラムの計算 */ void calcHistogram(); protected: // Configuration variable declaration /*! * * - Name: b_max * - DefaultValue: 256 * - 明度の最大値 */ int m_b_max; /*! * * - Name: b_min * - DefaultValue: 10 * - 明度の最小値 */ int m_b_min; /*! * * - Name: s_min * - DefaultValue: 30 * - 彩度の最小値 */ int m_s_min; /*! * * - Name: m_img_height * - DefaultValue: 240 * - 画像の高さ */ int m_img_height; /*! * * - Name: m_img_width * - DefaultValue: 320 * - 画像の幅 */ int m_img_width; // DataInPort declaration TimedOctetSeq m_in_image; InPort<TimedOctetSeq> m_in_imageIn; // DataOutPort declaration TimedOctetSeq m_tracking_image; OutPort<TimedOctetSeq> m_tracking_imageOut; TimedFloatSeq m_displacement; OutPort<TimedFloatSeq> m_displacementOut; private: int dummy; IplImage* m_hsv; // HSV表色系用IplImage IplImage* m_hue; // HSV表色系のHチャンネル用IplImage IplImage* m_mask; // マスク画像用IplImage IplImage* m_backproject; // バックプロジェクション画像用IplImage CvHistogram * m_hist; // ヒストグラム処理用構造体 // 処理モード選択用フラグ int m_backproject_mode; // バックプロジェクション画像の表示/非表示用フラグ値 (0: 非表示, 1: 表示) // CamShiftトラッキング用変数 CvRect m_track_window; CvBox2D m_track_box; CvConnectedComp m_track_comp; // ヒストグラム用変数 int m_hdims; // ヒストグラムの次元数 float m_hranges_arr[2]; // ヒストグラムのレンジ float* m_hranges; // 初期化判定フラグ int m_init_flag; }; extern "C" { void ObjectTrackingInit(RTC::Manager* manager); void on_mouse( int event, int x, int y, int flags, void* param ); }; #endif // OBJECTTRACKING_H
図25は、USBCameraAcquire,Flip,ObjectTracking,SeqInコンポーネントの接続例です。
まず、USBCameraAcquireコンポーネントにてUSBカメラの画像を取得します。
次に、Flipコンポーネントにて左右を反転させます。
反転させている理由は、物体追跡コンポーネントの出力をジョイスティックとして使用する場合に、画像が鏡のように表示されていた方が操作しやすいためです。
次に、ObjectTrackingコンポーネントで、あらかじめ選択された追跡対象物の移動量をOutPort(displacement)から出力し、SeqInコンポーネントで移動量を表示します。
ビルド済みパッケージを下記からダウンロードできます。
拡張子を"zip_"としてますので、"zip"にリネームしてから解凍して下さい。