おまけ(物体追跡コンポーネント)

 // -*- 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