OGIMOノート

7歳の娘と、5歳の息子(脳性麻痺)を持った父親エンジニアの備忘録。自作の電子工作おもちゃ/リハビリ器具/ロボット関係の製作記録、勉強記録を残していきます

YOLO v3による一般物体認識をROS上で試してみた

f:id:motokiinfinity8:20190701230409j:plain

はじめに

前回の記事で取り上げた深度計測カメラD435 と 自己位置認識カメラT265

ogimotokin.hatenablog.com

これを使って、『息子と自動で鬼ごっこをするロボット』や『息子からひたすら逃げる立位支援ロボット』などを作りたいというモチベーションがでてきました! そのために必要なのは、ある程度正確な『息子の位置&距離を計測するか』、です。 息子個人を認識するのは次のステップで実施するとしても、まずは人認識をして人との距離情報をカメラから取得するというやり方です。 一番単純なのはOpenCVに搭載されている顔検出機能を使う手段ですが、息子は普段はズリバイなど地面と接している機会が多く、なかなか顔を見せてくれません。なので、背中を向いていてもしっかり認識してくれる手段が必要です。

そんな時、ちょうどROS上で比較的簡単に動く darknet_rosノードがあるという情報を聞いたので、息子認識に対してどの程度有用であるか、早速試してみたいなぁ、と思います

darknet_ros セットアップ

開発環境

今回は、GPU搭載の開発用ゲーミング用PCで実行してみます。
 ・CPU : i7-8750H
 ・GPU : GeForce GTX 1060
 ・OS : Ubuntu16.04 LTS
いずれはJetson Nanoに移植しますが、まずは高スペック機で動作確認。

インストール手順

以下のサイトを参考にインストールを進めました。
demura.net

$ git clone --recursive https://github.com/leggedrobotics/darknet_ros.git
$ cd ../../
$ catkin_make -DCMAKE_BUILD_TYPE=Release

しかし、下記エラーで停止

CMake Error at darknet_ros/darknet_ros/CMakeLists.txt:161 (add_library):
Cannot find source file:
src/activation_layer.c
Tried extensions .c .C .c++ .cc .cpp .cxx .m .M .mm .h .hh .h++ .hm .hpp
.hxx .in .txx
CMake Error: CMake can not determine linker language for target: darknet_ros_lib
CMake Error: Cannot determine link language for target "darknet_ros_lib".

ん?なぜだろう…とよく見てみると

darknet_ros,/ darknet_ros,/CMakeList.txt内の ${DARKNET_PATH} が何故かルートファイル直下になっている。
原因が分からないが、これを回避するため、~/catkin_ws/src/darknet_ros/darknet/に置換することで回避した。
なんという力技(笑)

おかげで無事にcmakeは通りました!

次に、YOLO ver3のモデルのインストールは下記で実施します。

  wget http://pjreddie.com/media/files/yolov3.weights
  wget http://pjreddie.com/media/files/yolov3-voc.weights

上記は、/catkin_ws/src/darknet_ros/darknet_ros/yolo_network_config/weights/how_to_download_weights.txt にも記載されています。

動作確認

まず、使うカメラのトピック名を確認します!
D435で実施する場合であれば、RGB画像のトピック名は、
/camera/color/image_raw
になるはずなので、その場合は darknet_ros/darknet_ros/config/ros.yaml  を書き換えます。

[catkin_ws/src/darknet_ros/darknet_ros/config/ros.yaml]
subscribers:
  camera_reading:「
    topic: /camera/color/image_raw
    queue_size: 1

となっていればOKです。

その上で、一つ目のターミナル上で

roslaunch realsense2_camera rs_camera.launch

でRealsense D435カメラを起動。
その後に、darknet_ros を起動

roslaunch darknet_ros darknet_ros.launch

それで、認識画像も表示してくれます。これだと、1.0fps くらい。。。
雰囲気枯らして、おそらくCPU処理のみっぽそう

GPUでの動作確認

darknetでYOLOv3を動かしてみた。 - Qiita

を参考にして、darknet/Makefile を以下に変更

GPU=1
CUDNN=1
OPENCV=0
OPENMP=0
DEBUG=0

そして、再度ビルドすると……120fps!!!
GPUすごっ!!

実際に、息子で試してみると、顔が見えない側面や背面でも人物と認識していそう〜!
時々は、ネコとか認識してるが(笑)

ちなみに、認識結果は、
/darknet_ros/bounding_boxes
トピックに表示されるので、このトピックをサブスクライブすれば認識結果をいろいろと使えそうですね。

上記動画は、スコアの低い認識結果も表示しているのでビジーに見えるが、スコアの高いモノだけ表示する様にすれば
それなりに信頼度の高いデータのみを抽出できそうなので、そのあたりはチューニングしていきたいと思います。

D435で検出した人物との距離を測定する

上記の bouding_boxes 情報から人物のいるRGB画像上の座標を抽出し、その座標に対応した位置の深度画像から距離を抽出することとする。
D435 で使用するトピックは以下
・RGB画像 : /camera/color/image_raw
・深度画像 : /camera/aligned_depth_to_color/image_raw

深度画像は、/camera/depth/image_rect_raw' でも表示されるが、これだとRGB画像と座標相関がないため、上記にトピックを使用する。

サンプルコードは以下(Pythonで例を記載しています)

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

# This import is for general library
import os
import threading

# This import is for ROS integration
import rospy
from sensor_msgs.msg import Image,CameraInfo
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
from darknet_ros_msgs.msg import BoundingBoxes,BoundingBox
import cv2

class PersonDetector():
    def __init__(self):

        # cv_bridge handles
        self.cv_bridge = CvBridge()

        self.person_bbox = BoundingBox()

        # ROS PARAM
        self.m_pub_threshold = rospy.get_param('~pub_threshold', 0.40)

        # Subscribe
        sub_camera_rgb     =  rospy.Subscriber('/camera/color/image_raw', Image, self.CamRgbImageCallback)
        sub_camera_depth   =  rospy.Subscriber('/camera/aligned_depth_to_color/image_raw', Image, self.CamDepthImageCallback)
        sub_darknet_bbox   =  rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, self.DarknetBboxCallback)

        return

    def CamRgbImageCallback(self, rgb_image_data):
        try:
            rgb_image = self.cv_bridge.imgmsg_to_cv2(rgb_image_data, 'passthrough')
        except CvBridgeError, e:
            rospy.logerr(e)

        rgb_image = cv2.cvtColor(rgb_image, cv2.COLOR_BGR2RGB)

        # 人がいる場合
        if self.person_bbox.probability > 0.0 :

           # 一旦、BoundingBoxの中心位置の深度を取得 (今後改善予定)
            m_person_depth = self.m_depth_image[(int)(self.person_bbox.ymax+self.person_bbox.ymin)/2][(int)(self.person_bbox.xmax+self.person_bbox.xmin)/2]

            cv2.rectangle(rgb_image, (self.person_bbox.xmin, self.person_bbox.ymin), (self.person_bbox.xmax, self.person_bbox.ymax),(0,0,255), 2)
            rospy.loginfo('Class : person, Score: %.2f, Dist: %dmm ' %(self.person_bbox.probability, m_person_depth))
            text = "person " +('%dmm' % m_person_depth)
            text_top = (self.person_bbox.xmin, self.person_bbox.ymin - 10)
            text_bot = (self.person_bbox.xmin + 80, self.person_bbox.ymin + 5)
            text_pos = (self.person_bbox.xmin + 5, self.person_bbox.ymin)
            cv2.rectangle(rgb_image, text_top, text_bot, (0,0,0),-1)
            cv2.putText(rgb_image, text, text_pos, cv2.FONT_HERSHEY_SIMPLEX, 0.35, (255, 0, 255), 1)

        cv2.namedWindow("rgb_image")
        cv2.imshow("rgb_image", rgb_image)
        cv2.waitKey(10)
        cv2.normalize(self.m_depth_image, self.m_depth_image, 0, 32768, cv2.NORM_MINMAX)
        cv2.namedWindow("depth_image")
        cv2.imshow("depth_image", self.m_depth_image)
        cv2.waitKey(10)
        return


    def CamDepthImageCallback(self, depth_image_data):
        try:
            self.m_depth_image = self.cv_bridge.imgmsg_to_cv2(depth_image_data, 'passthrough')
        except CvBridgeError, e:
            rospy.logerr(e)
        self.m_camdepth_height, self.m_camdepth_width = self.m_depth_image.shape[:2]
        return

    def DarknetBboxCallback(self, darknet_bboxs):
        bboxs = darknet_bboxs.bounding_boxes
        person_bbox = BoundingBox()
        if len(bboxs) != 0 :
            for i, bb in enumerate(bboxs) :
                if bboxs[i].Class == 'person' and bboxs[i].probability >= self.m_pub_threshold:
                    person_bbox = bboxs[i]        
        self.person_bbox = person_bbox



if __name__ == '__main__':
    try:
        rospy.init_node('person_detector', anonymous=True)
        idc = PersonDetector()
        rospy.loginfo('idc Initialized')
        idc.start()
        rospy.spin()
        idc.finish()

    except rospy.ROSInterruptException:
        pass

その他参考情報

darknet_rosを使ったサンプルコード等は以下です。適宜参考にしていきます。

github.com
robot.isc.chubu.ac.jp