はじめに
前回の記事で取り上げた深度計測カメラ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処理のみっぽそう
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で例を記載しています）
import os
import threading
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):
self.cv_bridge = CvBridge()
self.person_bbox = BoundingBox()
self.m_pub_threshold = rospy.get_param('~pub_threshold', 0.40)
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 :
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
