チュートリアル(画像処理実習)

    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;
    };

 set(comp_srcs CircleTracking.cpp )
 set(standalone_srcs CircleTrackingComp.cpp)
 
 find_package(OpenCV REQUIRED) #追加

 # 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 <rtm/DataInPort.h>
 #include <rtm/DataOutPort.h>
 
 #include <opencv2/opencv.hpp> //追加

 private:
   cv::Mat m_imageBuff; //追加、入力画像を格納する変数
   cv::Mat m_outputBuff; //追加、円の情報を付加した画像を格納する変数
   int m_direction; //追加、移動ロボットの回転方向を格納する変数

 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;
     }
 
     //元のカラー画像をコピーして円の情報を画像に追加
     m_outputBuff = m_imageBuff.clone();
 
     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;
 }

 /usr/local/share/openrtm-1.2/components/c++/opencv-rtcs/OpenCVCamera/OpenCVCameraComp

 RTC::ReturnCode_t CircleTracking::onExecute(RTC::UniqueId /*ec_id*/)
 {
    (中略)
       m_outputBuff.create(cv::Size(m_image_in.width, m_image_in.height), CV_8UC3);
 
     }
     (以下を追加)
     //圧縮のフォーマットにより処理を分岐
     std::string format = (const char*)m_image_in.format;
     if (format == "jpeg" || format == "png")
     {
        std::vector<uchar> buff;
        int len = m_image_in.pixels.length();
        buff.resize(len);
        memcpy(&buff[0], &m_image_in.pixels[0], sizeof(unsigned char) * len);
        m_imageBuff = cv::imdecode(cv::Mat(buff), cv::IMREAD_COLOR);
     }
     else
     {
        std::memcpy(m_imageBuff.data,
            (void*)&(m_image_in.pixels[0]),
            m_image_in.pixels.length());
     }
     //以下の部分はコメントアウト。
     /*
     // InPortの画像データをm_imageBuffにコピー
     std::memcpy(m_imageBuff.data,
            (void *)&(m_image_in.pixels[0]),
            m_image_in.pixels.length());
     */