三次元画像処理

Learn

RGBDカメラを用いて三次元画像処理を行いましょう。

RGBDカメラについて

RGBDカメラとは、色(RGB)に加え、深度(Depth)情報を取得できるカメラのことです。 周りの環境を三次元の空間として認識することで、ロボットはより複雑にふるまうことができます。 比較的安価でよく利用されるRGBDカメラとして、Intel社製のRealSenseやMicrosoft社製のXtion(エクシオン)などがあります。

RealSense

今回はRGBDカメラとしてRealSenseD435を使用します。

ROSで用いる際には標準のラッパーを使用します。

roslaunch realsense2_camera rs_camera.launch

を実行すると、2種類のトピック

/camera/color/image_raw (RGB画像)
/camera/depth/image_raw (デプス画像)

が利用できるようになります。
これらのトピックはいずれもsensor_msgs/Image型です。

RealSenseはRGB画像モジュールとデプス画像モジュールが物理的に離れています。 このため、これら2つのトピックはいずれも画像データではあるものの、ピクセルの位置関係が対応しておらず、そのまま画像処理に利用することはできません。

RealSenseを使用するためのlaunchファイル(rs_camera.launch)を起動する際に “align_depth"パラメータを"true"に指定することで、デプス画像をRGB画像のピクセルに対応するように変換した /camera/aligned_depth_to_color/image_rawトピックが使用できるようになります。

ただし、roomba_bringupパッケージのbringup.launchファイルの

    <include file="$(find realsense2_camera)/launch/rs_camera.launch" if="$(arg realsense)">
        <arg name="align_depth" value="true"/>
    </include>

の箇所がこの操作に対応していため、今回は特別な操作をせずとも /camera/aligned_depth_to_color/image_rawトピックを使用できます。

物体検出

まずは3次元情報を扱わず、RGB画像/camera/color/image_rawのみを用いて画像検出を行ってみましょう。

以下は、 three-dementions_tutorialパッケージの object_detection.py です。

1. /camera/color/image_rawをsubscribeし、
2. 物体検出アルゴリズムであるYOLOv8に入力し、
3. 物体検出の結果をbounding boxとして描画し、
4. /detection_resultとしてpublish

の処理を行っています。

#!/usr/bin/env python3

import copy
from typing import List

import cv2
import rospy
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from ultralytics import YOLO
from ultralytics.engine.results import Results


class ObjectDetection:
    def __init__(self):
        rospy.init_node('object_detection', anonymous=True)

        # Publisher
        self.detection_result_pub = rospy.Publisher('/detection_result', Image, queue_size=10)

        # Subscriber
        rospy.Subscriber('/camera/color/image_raw', Image, self.callback_rgb)

        self.bridge = CvBridge()
        self.rgb_image = None

        self.model = YOLO('yolov8n.pt')

    def callback_rgb(self, data):
        cv_array = self.bridge.imgmsg_to_cv2(data, 'bgr8')
        self.rgb_image = cv_array

    def process(self):
        while not rospy.is_shutdown():
            if self.rgb_image is None:
                continue

            results: List[Results] = self.model.predict(self.rgb_image)

            # plot bounding box
            tmp_image = copy.deepcopy(self.rgb_image)
            for result in results:
                boxes = result.boxes.cpu().numpy()
                names = result.names
                for xyxy, conf, cls in zip(boxes.xyxy, boxes.conf, boxes.cls):
                    if conf < 0.5:
                        continue
                    x1, y1, x2, y2 = map(int, xyxy[:4])
                    cls_pred = cls
                    tmp_image = cv2.rectangle(tmp_image, (x1, y1), (x2, y2), (0, 255, 0), 3)
                    tmp_image = cv2.putText(tmp_image, names[cls_pred], (x1, y1), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)

            # publish image
            detection_result = self.bridge.cv2_to_imgmsg(tmp_image, "bgr8")
            self.detection_result_pub.publish(detection_result)


if __name__ == '__main__':
    od = ObjectDetection()
    try:
        od.process()
    except rospy.ROSInitException:
        pass

画像データを物体検出モデルに入力として渡すためには、その画像データの型がnp.ndarray型である必要があります。 そのため、コールバック関数で、受け取ったsensor_msgs/Image型の画像データをnp.ndarray型に変換しています。

cv_array = self.bridge.imgmsg_to_cv2(data, 'bgr8')

の部分がこの処理に対応します。
subscriberを宣言するときにコールバック関数を指定して、 subscribeしたデータをこの関数に渡すという基本的な処理の流れは、 scanなど他のトピックを扱うsubscriberと同じです。

ここで、YOLOの推論部分をコールバック関数内で行っていないことに注意しましょう。 一見、新しいデータが入ってくるときのみに推論を回すことは合理的に見えますが、 センサの入力に対してコールバック関数内の処理が重いと処理するデータが最新のものからどんどん遅れてしまいます。 コールバック関数はセンサデータの最低限の処理にとどめ、重い処理は分けて書くことを意識しましょう。

また、ここでは既存の物体検出モジュールを使用しましたが、PyTorchなどで自作したモデルも同様の枠組みで利用することができます。

三次元画像処理

次に、RGB画像とデプス画像を統合し、検出した物体までの距離を測定してみましょう。

/camera/color/image_raw (RGB画像) /camera/aligned_depth_to_color/image_raw (整列されたDepth画像)

はピクセル同士が対応するように処理されてはいるものの、 パブリッシュされた時刻は独立しているため、 併せて使用するには時刻の同期を行う必要があります。

画像の時刻同期にはmessage_filtersがよく使われます。

message_filters.ApproximateTimeSynchronizerを使い、以下のようにSubscriberを作成します。

#!/usr/bin/env python3

import copy
from typing import List

import cv2
import message_filters
import rospy
from cv_bridge import CvBridge
from sensor_msgs.msg import Image

class DetectionDistance:
    def __init__(self):
        rospy.init_node('detection_distance', anonymous=True)

        # Publisher
        self.detection_result_pub = rospy.Publisher('/detection_result', Image, queue_size=10)

        # Subscriber
        rgb_sub = message_filters.Subscriber('/camera/color/image_raw', Image)
        depth_sub = message_filters.Subscriber('/camera/aligned_depth_to_color/image_raw', Image)
        message_filters.ApproximateTimeSynchronizer([rgb_sub, depth_sub], 10, 1.0).registerCallback(self.callback_rgbd)

        self.bridge = CvBridge()
        self.rgb_image, self.depth_image = None, None

    def callback_rgbd(self, data1, data2):
        cv_array = self.bridge.imgmsg_to_cv2(data1, 'bgr8')
        cv_array = cv2.cvtColor(cv_array, cv2.COLOR_BGR2RGB)
        self.rgb_image = cv_array

        cv_array = self.bridge.imgmsg_to_cv2(data2, 'passthrough')
        self.depth_image = cv_array
# 後略

この例では、
/camera/color/image_rawと
/camera/aligned_depth_to_color/image_rawの
トピックを同時(1.0秒までのずれを許容する)に受け取った場合のみ、 2つの画像データをコールバック関数callback_rgbdに渡します。

それでは、three-dementions_tutorial パッケージのdetection_distance.py を見てみましょう。
物体を検出し、その物体までの距離を測定するスクリプトです。

#!/usr/bin/env python3

import copy
from typing import List

import cv2
import message_filters
import rospy
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from ultralytics import YOLO
from ultralytics.engine.results import Results


class DetectionDistance:
    def __init__(self):
        rospy.init_node('detection_distance', anonymous=True)

        # Publisher
        self.detection_result_pub = rospy.Publisher('/detection_result', Image, queue_size=10)

        # Subscriber
        rgb_sub = message_filters.Subscriber('/camera/color/image_raw', Image)
        depth_sub = message_filters.Subscriber('/camera/aligned_depth_to_color/image_raw', Image)
        message_filters.ApproximateTimeSynchronizer([rgb_sub, depth_sub], 10, 1.0).registerCallback(self.callback_rgbd)

        self.bridge = CvBridge()
        self.rgb_image, self.depth_image = None, None

        self.model = YOLO('yolov8n.pt')

    def callback_rgbd(self, data1, data2):
        cv_array = self.bridge.imgmsg_to_cv2(data1, 'bgr8')
        cv_array = cv2.cvtColor(cv_array, cv2.COLOR_BGR2RGB)
        self.rgb_image = cv_array

        cv_array = self.bridge.imgmsg_to_cv2(data2, 'passthrough')
        self.depth_image = cv_array

    def process(self):
        while not rospy.is_shutdown():
            if self.rgb_image is None:
                continue

            # inference
            tmp_image = copy.copy(self.rgb_image)

            results: List[Results] = self.model.predict(self.rgb_image, verbose=False)

            # plot bouding box
            for result in results:
                boxes = result.boxes.cpu().numpy()
                names = result.names
                if len(boxes.xyxy) == 0:
                    continue
                x1, y1, x2, y2 = map(int, boxes.xyxy[0][:4])
                cls_pred = boxes.cls[0]
                tmp_image = cv2.rectangle(tmp_image, (x1, y1), (x2, y2), (0, 255, 0), 3)
                tmp_image = cv2.putText(tmp_image, names[cls_pred], (x1, y1), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
                cx, cy = (x1+x2)//2, (y1+y2)//2
                print(names[cls_pred], self.depth_image[cy][cx]/1000, "m")

            # publish image
            tmp_image = cv2.cvtColor(tmp_image, cv2.COLOR_RGB2BGR)
            detection_result = self.bridge.cv2_to_imgmsg(tmp_image, "bgr8")
            self.detection_result_pub.publish(detection_result)


if __name__ == '__main__':
    dd = DetectionDistance()
    try:
        dd.process()
    except rospy.ROSInitException:
        pass

基本的には物体検出のスクリプトと同じですが、

cx, cy = (x1+x2)//2, (y1+y2)//2
print(names[cls_pred], self.depth_image[cy][cx]/1000, "m")

でbounding boxの中心座標を変換し、対応する距離をメートル単位で表示しています。

整列されたデプス画像を用いているため、RGB画像に基づき算出した座標をそのまま指定できます。

点群の作成

上の例ではRGB画像とDepth画像を用いて、物体位置の代表となる点とロボット位置の関係を扱うことができました。

しかし、代表となる点以外の深度データも三次元空間に直接マッピングできると、 物体の大きさや形状といった情報も扱うことができ、 環境情報をより直感的・統一的に扱うことができるように思われます。

そこでDepth画像から点群と呼ばれるデータを作成することを考えます。

点群とは三次元座標値 (X, Y, Z) で構成された点の集まりのことです。各点の情報として、三次元座標値に加え色の情報 (R, G, B) が加わることもあります。 デプス画像はカメラの内部パラメータを用いることによって点群データに変換することができます。(参考)

今回は、デプス画像を点群データに変換するためのROSの外部パッケージである depth_image_proc を使用して点群を作成します。

外部パッケージは~/catkin_ws/src等のワークスペースに配置し、ビルドしパスを通すことで簡単に使用できます。

depth_image_procのwikiを参考に以下のようなlaunchファイルを作成しました。

<?xml version="1.0"?>
<launch>
  <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" />

  <node pkg="nodelet" type="nodelet" name="nodelet1"
        args="load depth_image_proc/point_cloud_xyz nodelet_manager">
    <remap from="camera_info" to="/camera/color/camera_info"/>
    <remap from="image_rect" to="/camera/aligned_depth_to_color/image_raw"/>
    <remap from="points" to="/camera/depth/points"/>
  </node>
</launch>

このlaunchファイルを実行すると
/camera/color/camera_infoと
/camera/aligned_depth_to_color/image_rawを
subscribeし、
/camera/depth/pointsをpublishするノードが作成されます。

/camera/color/camera_infoは sensor_msgs/CameraInfo型のトピックです。 カメラパラメータやフレームid、タイムスタンプなどの情報を保持しており、点群の変換に利用されます。

/camera/aligned_depth_to_color/image_rawはRGB画像に合わせて整列されたDepth画像であるため、 /camera/depth/camera_infoではなく
/camera/color/camera_infoを指定しています。

(開発PC)(docker)# roslaunch three-dimensions_tutorial depth2pc.launch

を実行し、/camera/depth/pointsトピックをrvizで可視化をすると三次元空間に点群データが表示されているのが確認できます。

演習

(jetson)$ ./RUN-DOCKER-CONTAINER.sh
(jetson)(docker)# roslaunch roomba_bringup bringup.launch
(開発PC)$ ./RUN-DOCKER-CONTAINER.sh 192.168.10.7x

(開発PC)(docker)# rviz

(開発PC)(docker)# cd catkin_ws; catkin_make; source devel/setup.bash
(開発PC)(docker)# rosrun three-dimensions_tutorial object_detection.py
# rvizで`/detection_result`を表示し結果を確認してみよう。
(開発PC)(docker)# rosrun three-dimensions_tutorial detection_distance.py

(開発PC)(docker)# cd ~/external_catkin_ws/src 
(開発PC)(docker)# git clone https://github.com/ros-perception/image_pipeline
(開発PC)(docker)# cd ../; catkin build; source devel/setup.bash
(開発PC)(docker)# cd ~/roomba_hack/catkin_ws; source devel/setup.bash
(開発PC)(docker)# roslaunch three-dimensions_tutorial depth2pc.launch
(開発PC)(docker)# roslaunch navigation_tutorial navigation.launch