はじめに
前回の記事で取り上げた深度計測カメラD435 と 自己位置認識カメラT265
これを使って、『息子と自動で鬼ごっこをするロボット』や『息子からひたすら逃げる立位支援ロボット』などを作りたいというモチベーションがでてきました！ そのために必要なのは、ある程度正確な『息子の位置&距離を計測するか』、です。 息子個人を認識するのは次のステップで実施するとしても、まずは人認識をして人との距離情報をカメラから取得するというやり方です。 一番単純なのは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すごっ！！
息子向けの生活サポートロボット製作に向け、息子認識システムの基礎検討中。 ROS上で Yolo v3を動かしてAI物体認識をさせてみた。
— おぎ-モトキ (@ogimotoki) 2019年6月20日
ノートPCだと、高速に認識！
単純な顔認識と違い、横や後ろでも人認識するので、ズリバイだらけの息子もこれなら検出できる！ ただし、時々ニャンコになっとるが（笑） pic.twitter.com/5kKwd7dl7b
実際に、息子で試してみると、顔が見えない側面や背面でも人物と認識していそう〜！
時々は、ネコとか認識してるが(笑)
ちなみに、認識結果は、
/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
