ROSによる画像処理(Python, Kinect版)

目的

Turtlebotに搭載されているKinectは,カラー画像と距離が取得できるセンサーです.本演習では,ROSを介してKinectより得られる画像を用いたプログラミング方法について学びます.なお,C++言語による画像処理についてはこちらのページを参照してください.

Kinectからの画像取得と表示

VM環境下でubuntuを動かしている人はKinectに接続できません.チーム単位で本日の演習に取り組んでください.2017年まではopenniパッケージを使用していましたが,エラーが多発するためfreenectパッケージを使用します.openniパッケージを使用したい方はこちらのページを参照してください
まずは,既存のパッケージから 画像を取得,表示しましょう.kinectに接続し,画像のトピックを獲得するために下記のコマンドを実行します.

$ roslaunch freenect_launch freenect.launch

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

$ rosrun rviz rviz

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

$ rosrun rqt_image_view rqt_image_view

ウィンドウが立ち上がるので,GUIから表示したい画像トピックを選択してください.カラー画像は/camera/rgb/image_color,距離画像は/camera/depth/imageとなります.

Kinectによる画像処理

1. パッケージの作成
kinectを扱う場合には,依存パッケージとして下記を指定してください.なお,本ページではパッケージの名前をdepth_estimaterとして話を進めます.

cv_bridge rospy sensor_msgs std_msgs

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

#! /usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import cv2
import sys
import message_filters
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image

class depth_estimater:
    WIDTH = 50
    HEIGHT = 25

    def __init__(self):

        rospy.init_node('depth_estimater', anonymous=True)
        self.bridge = CvBridge()
        sub_rgb = message_filters.Subscriber("camera/rgb/image_color",Image)
        sub_depth = message_filters.Subscriber("camera/depth/image",Image)
        self.mf = message_filters.ApproximateTimeSynchronizer([sub_rgb, sub_depth], 100, 10.0)
        self.mf.registerCallback(self.ImageCallback)

    def ImageCallback(self, rgb_data , depth_data):
        try:
            color_image = self.bridge.imgmsg_to_cv2(rgb_data, 'passthrough')
            depth_image = self.bridge.imgmsg_to_cv2(depth_data, 'passthrough')
        except CvBridgeError, e:
            rospy.logerr(e)

        color_image.flags.writeable = True
        color_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB) 
        h, w, c = color_image.shape

        x1 = (w / 2) - self.WIDTH
        x2 = (w / 2) + self.WIDTH
        y1 = (h / 2) - self.HEIGHT
        y2 = (h / 2) + self.HEIGHT
        sum = 0.0

        for i in range(y1, y2):
            for j in range(x1, x2):
                color_image.itemset((i, j, 0), 0)
                color_image.itemset((i, j, 1), 0)
                #color_image.itemset((100,100,2), 0)

                if depth_image.item(i,j) == depth_image.item(i,j):
                    sum += depth_image.item(i,j)

        ave = sum / ((self.WIDTH * 2) * (self.HEIGHT * 2))
        print("%f [m]" % ave)

        cv2.normalize(depth_image, depth_image, 0, 1, cv2.NORM_MINMAX)
        cv2.namedWindow("color_image")
        cv2.namedWindow("depth_image")
        cv2.imshow("color_image", color_image)
        cv2.imshow("depth_image", depth_image)
        cv2.waitKey(10)

if __name__ == '__main__':
    try:
        de = depth_estimater()
        rospy.spin()
    except rospy.ROSInterruptException: pass

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

roslaunch openni_launch openni.launch

次に,rosrunにより作成したノードを起動しましょう.ノードを起動する前に,chmodコマンドで作成したプログラムに対して実行権限を付与しましょう.下記に作成したノードを実行した例を示します.

Screenshot_from_2015-06-12 08:13:11

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

import rospy
import cv2
import sys
import message_filters
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image

必要なモジュールを読み込んでいます.OpenCVを使用する場合にはcv2の他に,ROSのデータからOpenCVで扱えるデータ形式に変換する必要があるためCvBridgeを読み込む必要があります.

class depth_estimater:
    WIDTH = 50
    HEIGHT = 25

depth_estimaterというクラスを定義しています.また,クラス変数としてWIDTHとHEIGHTを宣言し,それぞれに値を代入しています.これら2つの変数はROI領域の大きさを表しています.実際には,(WIDTH*2)x(HEIGHT*2)の画素数となるので注意してください.

    def __init__(self):
 
        rospy.init_node('depth_estimater', anonymous=True)
        self.bridge = CvBridge() 
        sub_rgb = message_filters.Subscriber("camera/rgb/image_color",Image)
        sub_depth = message_filters.Subscriber("camera/depth/image",Image)
        self.mf = message_filters.ApproximateTimeSynchronizer([sub_rgb, sub_depth], 100, 10.0)
        self.mf.registerCallback(self.ImageCallback)

depth_estimaterというクラスを定義して画像データを購読するための設定をしています.ROSのデータ形式からOpenCVで扱えるデータ形式に変換するためにCvBridgeを使用します.18行目ではCvBridgeを使う準備をしています.
20行目から23行目は,カラー画像と距離画像を購読するための準備をしています.2つの画像を購読するためにmessage_filtersを利用しています.message_filtersは,複数のトピックの同期を取るためのパッケージです.20,21行目で購読したいトピックを設定し,22行目で同期を取る設定,23行目で2つのトピックを購読した際に呼び出したい関数を設定しています.22行目のmessage_filters.ApproximateTimeSynchronizerの第一引数で同期を取りたいトピックの設定,第二引数でキューのサイズ(データを蓄積するサイズ),第三引数で遅延を許容する時間を設定します.カメラのフレームレートが低い場合や,通信帯域が狭い,通信量が多い場合にはトピックを購読できるタイミングにズレが生じやすいので,第三引数の値を大きめにしておくと(時間的なズレは大きいですが)購読できる頻度が高くなります.

def ImageCallback(self, rgb_data , depth_data):
        try:
            color_image = self.bridge.imgmsg_to_cv2(rgb_data, 'passthrough')
            depth_image = self.bridge.imgmsg_to_cv2(depth_data, 'passthrough')
        except CvBridgeError, e:
            rospy.logerr(e)
 
        color_image.flags.writeable = True
        color_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)
        h, w, c = color_image.shape
 
        x1 = (w / 2) - self.WIDTH
        x2 = (w / 2) + self.WIDTH
        y1 = (h / 2) - self.HEIGHT
        y2 = (h / 2) + self.HEIGHT
        sum = 0.0
 
        for i in range(y1, y2):
            for j in range(x1, x2):
                color_image.itemset((i, j, 0), 0)
                color_image.itemset((i, j, 1), 0)
                #color_image.itemset((100,100,2), 0)
 
                if depth_image.item(i,j) == depth_image.item(i,j):
                    sum += depth_image.item(i,j)
 
        ave = sum / ((self.WIDTH * 2) * (self.HEIGHT * 2))
        print("%f [m]" % ave)
 
        cv2.normalize(depth_image, depth_image, 0, 1, cv2.NORM_MINMAX)
 
        cv2.namedWindow("color_image")
        cv2.namedWindow("depth_image")
        cv2.imshow("color_image", color_image)
        cv2.imshow("depth_image", depth_image)
        cv2.waitKey(10)

カラー画像と距離画像を購読した際には上記の関数が実行されます.順番にコードを見ていきます.

        try:
            color_image = self.bridge.imgmsg_to_cv2(rgb_data, 'passthrough')
            depth_image = self.bridge.imgmsg_to_cv2(depth_data, 'passthrough')
        except CvBridgeError, e:
            rospy.logerr(e)

購読したカラー画像と距離画像のデータは,ROSのメッセージで定義された形式です.このままではOpenCVでは扱えないため,OpenCVで扱うためにimgmsg_to_cv2()によってデータ形式を変換しています.imgmsg_to_cv2()の第一引数は画像データ,第二引数はエンコーディング方式です.passthroughとすると,第一引数で与えた画像データに対応した形式に変換されます.

        color_image.flags.writeable = True
        color_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)
        h, w, c = color_image.shape
 
        x1 = (w / 2) - self.WIDTH
        x2 = (w / 2) + self.WIDTH
        y1 = (h / 2) - self.HEIGHT
        y2 = (h / 2) + self.HEIGHT
        sum = 0.0

変換した画像データは書き換えができないようになっているため,31行目でカラー画像に対して書き込み権限を与えています.32行目ではカラー画像の縦,横の画素数,チャンネル数をそれぞれh,w,cに代入しています.34行目から37行目ではROI領域の大きさを設定しています.

        for i in range(y1, y2):
            for j in range(x1, x2):
                color_image.itemset((i, j, 0), 0)
                color_image.itemset((i, j, 1), 0)
                #color_image.itemset((100,100,2), 0)
 
                if depth_image.item(i,j) == depth_image.item(i,j):
                    sum += depth_image.item(i,j)

カラー画像と距離画像のROI領域の画素に対して処理しています.42,43行目ではitemset()を用いて(i, j)画素の青と緑のチャンネルに対して0を代入しています.チャンネルの順番は青(0),緑(1),赤(2)です.
47行目にてROI領域の距離値の総和を計算しています.ただし,距離が取得できない場合にはdepth_image.item(i,j)からはNaN(Not a Number,非数という意味)が返ってくるため,46行目でNaNの判定を行い,NaN以外の距離値の総和を求めるようにしています.NaNの判定には幾つかの方法がありますが,今回は自身(depth_image.item(i,j))と比較してTrueであれば距離値,FlaseであればNaNとして判定しています.

        ave = sum / ((self.WIDTH * 2) * (self.HEIGHT * 2))
        print("%f [m]" % ave)

計算した赤枠内の距離和を用いて平均距離を計算し表示しています.

        cv2.normalize(depth_image, depth_image, 0, 1, cv2.NORM_MINMAX)
        cv2.namedWindow("color_image")
        cv2.namedWindow("depth_image")
        cv2.imshow("color_image", color_image)
        cv2.imshow("depth_image", depth_image)
        cv2.waitKey(10)

最後にROI領域を赤くマスクした画像と距離画像を表示しています.
ただし,depth_imageは1画素を32ビットの浮動小数点方式で距離値を表現しているため,このままでは画像として正しく表示することはできません.そこで,normalize()によって距離値を0から1の範囲に正規化をしています.imshow()では,画像が32ビットの浮動小数点方式で表現されている場合,画素値(ここでは距離値)に対して255が掛けられます.すなわち0から1の範囲に正規化された値は,0から255の値に変換されて表示されます.

if __name__ == '__main__':
    try:
        de = depth_estimater()
        rospy.spin()
    except rospy.ROSInterruptException: pass

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

課題

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