Kinectによる画像処理

目的

Turtlebotに搭載されているKinectは,カラー画像と距離が取得できるセンサーです.本演習では,ROSを介してKinectより得られる画像を用いたプログラミング方法について学びます.

KINECTが接続できない場合

VM環境下でubuntuを動かしている人は,Kinectに接続できません.チーム単位で本日の演習に取り組んでください.Kinectに接続したPCからトピックを購読することはできます.VM環境でubuntuを起動していないけど,Kinectが動作しない人はこちらのページを参考にしてください.

Kinectからの画像取得と表示

まずは,既存のパッケージから 画像を取得,表示しましょう.kinectに接続し,画像のトピックを獲得するために下記のコマンドを実行します.

$ roslaunch openni_launch openni.launch

openni_launchがない場合にはこちらのページを参考にしてください.

次に画像を表示する2つの方法について説明します.1つ目は,rvizを用いた画像を表示する方法です.下記のコマンドを実行してrvizを立ち上げましょう.

$ rosrun rviz rviz

2つ目はimage_viewを使用する方法です.端末で下記を実行しましょう.

$ rosrun image_view image_view image:=/camera/rgb/image_color
$ rosrun image_view image_view image:=/camera/depth/image

1行目はカラー画像(RGB画像),2行目は距離画像(D画像)を表示します.

Kinectによる画像処理

1. パッケージの作成
kinectを扱う場合には,依存パッケージとして下記を指定してください.

cv_bridge image_transport roscpp sensor_msgs std_msgs

2. プログラミング
それでは,kinectから得られたRGB画像とD画像を処理するプログラムを書きます.今回は,RGB画像の注目する領域(ROI領域)を赤く表示し,そのROI領域の平均距離を計算するプログラムを作成します.

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#define WIDTH   50
#define HEIGHT  25

class depth_estimater{
public:
    depth_estimater();
    ~depth_estimater();
    void rgbImageCallback(const sensor_msgs::ImageConstPtr& msg);
    void depthImageCallback(const sensor_msgs::ImageConstPtr& msg);

private:
    ros::NodeHandle nh;
    ros::Subscriber sub_rgb, sub_depth;
};

depth_estimater::depth_estimater(){
    sub_rgb = nh.subscribe<sensor_msgs::Image>("/camera/rgb/image_color", 1, &depth_estimater::rgbImageCallback, this);
    sub_depth = nh.subscribe<sensor_msgs::Image>("/camera/depth/image", 1, &depth_estimater::depthImageCallback, this);
}

depth_estimater::~depth_estimater(){
}

void depth_estimater::rgbImageCallback(const sensor_msgs::ImageConstPtr& msg){

    int i, j;
    int x1, x2, y1, y2;
    int width = WIDTH;
    int height = HEIGHT;
    cv_bridge::CvImagePtr cv_ptr;

    try{
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }catch (cv_bridge::Exception& ex){
        ROS_ERROR("error");
        exit(-1);
    }

    cv::Mat &mat = cv_ptr->image;

    x1 = int(mat.cols / 2) - width;
    x2 = int(mat.cols / 2) + width;
    y1 = int(mat.rows / 2) - height;
    y2 = int(mat.rows / 2) + height;

    for(i = y1; i < y2; i++){
        for(j = x1; j < x2; j++){
            // 0 : blue, 1 : green, 2 : red.
            mat.data[i * mat.step + j * mat.elemSize() + 0] = 0;
            mat.data[i * mat.step + j * mat.elemSize() + 1] = 0;
            //mat.data[i * mat.step + j * mat.elemSize() + 2] = 0;
        }
    }

    cv::rectangle(cv_ptr->image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 0, 255), 3, 4);
    cv::imshow("RGB image", cv_ptr->image);
    cv::waitKey(10);
}

void depth_estimater::depthImageCallback(const sensor_msgs::ImageConstPtr& msg){

    int x1, x2, y1, y2;
    int i, j, k;
    int width = WIDTH;
    int height = HEIGHT;
    double sum = 0.0;
    double ave;
    cv_bridge::CvImagePtr cv_ptr;

    try{
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
    }catch (cv_bridge::Exception& ex){
        ROS_ERROR("error");
        exit(-1);
    }

    cv::Mat depth(cv_ptr->image.rows, cv_ptr->image.cols, CV_32FC1);
    cv::Mat img(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1);

    x1 = int(depth.cols / 2) - width;
    x2 = int(depth.cols / 2) + width;
    y1 = int(depth.rows / 2) - height;
    y2 = int(depth.rows / 2) + height;

    for(i = 0; i < cv_ptr->image.rows;i++){
        float* Dimage = cv_ptr->image.ptr<float>(i);
        float* Iimage = depth.ptr<float>(i);
        char* Ivimage = img.ptr<char>(i);
        for(j = 0 ; j < cv_ptr->image.cols; j++){
            if(Dimage[j] > 0.0){
                Iimage[j] = Dimage[j];
                Ivimage[j] = (char)(255*(Dimage[j]/5.5));
            }else{
            }

            if(i > y1 && i < y2){
                if(j > x1 && j < x2){
                    if(Dimage[j] > 0.0){
                        sum += Dimage[j];
                    }
                }
            }
        }
    }

    ave = sum / ((width * 2) * (height * 2));
    ROS_INFO("depth : %f [m]", ave);

    cv::imshow("DEPTH image", img);
    cv::waitKey(10);
}

int main(int argc, char **argv){
    ros::init(argc, argv, "depth_estimater");

    depth_estimater depth_estimater;

    ros::spin();
    return 0;
}

3. 実行
作成したプログラムを実行します.kinectから画像を取得するためには,先ほどのlaunchファイルをroslaunchします.

roslaunch openni_launch openni.launch

次に,rosrunにより作成したノードを起動しましょう.下記に作成したノードを実行した例を示します.

Screenshot_from_2015-06-12 08:13:11

4. プログラムの内容
それでは,プログラムの内容を確認しましょう.

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

必要なヘッダファイルを読み込んでいます.Open CVを使用する場合には,多くのヘッダファイルが必要になるので,全て記述しましょう.

#define WIDTH   50
#define HEIGHT  25

赤枠の大きさです.実際には,(WIDTH*2)x(HEIGHT*2)の画素数となるので注意してください.

class depth_estimater{
public:
    depth_estimater();
    ~depth_estimater();
    void rgbImageCallback(const sensor_msgs::ImageConstPtr& msg);
    void depthImageCallback(const sensor_msgs::ImageConstPtr& msg);
 
private:
    ros::NodeHandle nh;
    ros::Subscriber sub_rgb, sub_depth;
};

depth_estimaterというクラスを定義し,幾つかのクラスメンバを宣言しています.カラー画像を購読した際にはrgbImageCallback関数,距離画像を購読した際にはdepthImageCallback関数が実行されます.

void depth_estimater::rgbImageCallback(const sensor_msgs::ImageConstPtr& msg){
 
    int i, j;
    int x1, x2, y1, y2;
    int width = WIDTH;
    int height = HEIGHT;
    cv_bridge::CvImagePtr cv_ptr;
 
    try{
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }catch (cv_bridge::Exception& ex){
        ROS_ERROR("error");
        exit(-1);
    }
 
    cv::Mat &mat = cv_ptr->image;
 
    x1 = int(mat.cols / 2) - width;
    x2 = int(mat.cols / 2) + width;
    y1 = int(mat.rows / 2) - height;
    y2 = int(mat.rows / 2) + height;
 
    for(i = y1; i < y2; i++){
        for(j = x1; j < x2; j++){
            // 0 : blue, 1 : green, 2 : red.
            mat.data[i * mat.step + j * mat.elemSize() + 0] = 0;
            mat.data[i * mat.step + j * mat.elemSize() + 1] = 0;
            //mat.data[i * mat.step + j * mat.elemSize() + 2] = 0;
        }
    }
 
    cv::rectangle(cv_ptr->image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 0, 255), 3, 4);
    cv::imshow("RGB image", cv_ptr->image);
    cv::waitKey(10);
}

カラー画像を購読した際には上記の関数が実行されます.詳細を見ましょう.

    int i, j;
    int x1, x2, y1, y2;
    int width = WIDTH;
    int height = HEIGHT;
    cv_bridge::CvImagePtr cv_ptr;

変数を宣言しています.cv_ptrは画像を扱うためのものです.

    try{
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }catch (cv_bridge::Exception& ex){
        ROS_ERROR("error");
        exit(-1);
    }
 
    cv::Mat &mat = cv_ptr->image;

画像を購読し,BGR8形式(1チャンネル(青,緑,赤のどれか1つ)につき8ビット(0~255階調))のカラー画像に変換し,cv_ptrに代入しています.ここで注意すべき点は,チャンネルの順番がBGR,すなわち青,緑,赤の順になっている点です.また,画像を購読できない場合には,エラーを出力します.そして,cv_ptrをmatに代入しています.

    x1 = int(mat.cols / 2) - width;
    x2 = int(mat.cols / 2) + width;
    y1 = int(mat.rows / 2) - height;
    y2 = int(mat.rows / 2) + height;

赤枠の大きさを設定しています.

    for(i = y1; i < y2; i++){
        for(j = x1; j < x2; j++){
            // 0 : blue, 1 : green, 2 : red.
            mat.data[i * mat.step + j * mat.elemSize() + 0] = 0;
            mat.data[i * mat.step + j * mat.elemSize() + 1] = 0;
            //mat.data[i * mat.step + j * mat.elemSize() + 2] = 0;
        }
    }

赤枠内の画素にアクセスし,青と緑のチャンネルに0を代入しています.チャンネルの順番は,青,緑,赤です.画素にアクセスしているプログラムは3行ありますが,先ほどのチャンネルの順番となっているので注意しましょう.画素にアクセスする方法は幾つかあります.アクセス方法によっては,時間がかかる場合があるので注意が必要です.アクセスの仕方についてはこちらのページを参考にしましょう.

    cv::rectangle(cv_ptr->image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 0, 255), 3, 4);
    cv::imshow("RGB image", cv_ptr->image);
    cv::waitKey(10);

最後に,rectangle関数で枠の外側を赤色で囲っています.そして,imshow関数により画像を表示しています.waitKey関数はキーが押されるまで待機する関数です.今回はキー入力を必要としませんが,画像を表示する場合には必要な関数なので記述しましょう.

void depth_estimater::depthImageCallback(const sensor_msgs::ImageConstPtr& msg){

    int x1, x2, y1, y2;
    int i, j, k;
    int width = WIDTH;
    int height = HEIGHT;
    double sum = 0.0;
    double ave;
    cv_bridge::CvImagePtr cv_ptr;

    try{
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
    }catch (cv_bridge::Exception& ex){
        ROS_ERROR("error");
        exit(-1);
    }

    cv::Mat depth(cv_ptr->image.rows, cv_ptr->image.cols, CV_32FC1);
    cv::Mat img(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1);

    x1 = int(depth.cols / 2) - width;
    x2 = int(depth.cols / 2) + width;
    y1 = int(depth.rows / 2) - height;
    y2 = int(depth.rows / 2) + height;

    for(i = 0; i < cv_ptr->image.rows;i++){
        float* Dimage = cv_ptr->image.ptr<float>(i);
        float* Iimage = depth.ptr<float>(i);
        char* Ivimage = img.ptr<char>(i);
        for(j = 0 ; j < cv_ptr->image.cols; j++){
            if(Dimage[j] > 0.0){
                Iimage[j] = Dimage[j];
                Ivimage[j] = (char)(255*(Dimage[j]/5.5));
            }else{
            }

            if(i > y1 && i < y2){
                if(j > x1 && j < x2){
                    if(Dimage[j] > 0.0){
                        sum += Dimage[j];
                    }
                }
            }
        }
    }

    ave = sum / ((width * 2) * (height * 2));
    ROS_INFO("depth : %f [m]", ave);

    cv::imshow("DEPTH image", img);
    cv::waitKey(10);
}

距離画像を購読した際には上記の関数が実行されます.もう少し詳細を見ましょう.

    int x1, x2, y1, y2;
    int i, j, k;
    int width = WIDTH;
    int height = HEIGHT;
    double sum = 0.0;
    double ave;
    cv_bridge::CvImagePtr cv_ptr;

カラー画像と同様に変数を宣言しています.

    try{
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
    }catch (cv_bridge::Exception& ex){
        ROS_ERROR("error");
        exit(-1);
    }

上記もカラー画像とほぼ同じ内容です.異なる点は,TYPE_32FC1形式に変換している点です.TYPE_32FC1は,1チャンネル浮動小数点数(1つの画素に32ビット)を表しています.

    cv::Mat depth(cv_ptr->image.rows, cv_ptr->image.cols, CV_32FC1);
    cv::Mat img(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1);

指定した大きさの配列を確保しています.CV_8UC1は,先ほど出てきたBGR8と同じです.

    x1 = int(depth.cols / 2) - width;
    x2 = int(depth.cols / 2) + width;
    y1 = int(depth.rows / 2) - height;
    y2 = int(depth.rows / 2) + height;

枠の大きさを設定しています.

        for(i = 0; i < cv_ptr->image.rows;i++){
        float* Dimage = cv_ptr->image.ptr<float>(i);
        float* Iimage = depth.ptr<float>(i);
        char* Ivimage = img.ptr<char>(i);
        for(j = 0 ; j < cv_ptr->image.cols; j++){
            if(Dimage[j] > 0.0){
                Iimage[j] = Dimage[j];
                Ivimage[j] = (char)(255*(Dimage[j]/5.5));
            }else{
            }
 
            if(i > y1 && i < y2){
                if(j > x1 && j < x2){
                    if(Dimage[j] > 0.0){
                        sum += Dimage[j];
                    }
                }
            }
        }
    }
    ave = sum / ((width * 2) * (height * 2));
    ROS_INFO("depth : %f [m]", ave);

枠内の距離値の合計を計算しています.その際,もし距離が0[m]以下であれば,除外しております.先ほどとは画像のアクセスの仕方が異なります.
CV_32FC1型では,dataメソッドを使用して画素にアクセスすることができないため,ptrメソッドを使用しています.また,距離画像を可視化するために,100行目では距離値を取得できる最大距離で割り,255を掛けています.255は8ビットで表現できる最大の数です.最後に,114行目で枠内の画素数で割り,計算した平均距離を次の行で出力しています.

    cv::imshow("DEPTH image", img);
    cv::waitKey(10);

カラー画像を購読する関数と同様に,可視化した距離画像を表示しています.

int main(int argc, char **argv){
    ros::init(argc, argv, "depth_estimater");
 
    depth_estimater depth_estimater;
 
    ros::spin();
    return 0;
}

以前と変わらない処理なので説明を省略します.

問題

Turtlebotが前進している際に,Turtlebotと前方の壁までの距離が1.0m以下になったら停止しなさい.