Python中ROS和OpenCV結(jié)合處理圖像問題

第Python中ROS和OpenCV結(jié)合處理圖像問題目錄一、安裝ROS-OpenCV二、簡單案例分析1.usb_cam.launch2.cv_bridge_test.py3.rqt_image_view三、CvBridge相關(guān)API1.imgmsg_to_cv2()2.cv2_to_imgmsg()四、利用ROS+OpenCV實現(xiàn)人臉檢測案例1.usb_cam.launch2.face_detector.launch2.1launch2.2face_detector.py2.3兩個xml文件3.rqt_image_view五、利用ROS+OpenCV實現(xiàn)幀差法物體追蹤1.usb_cam.launch2.motion_detector.launch2.1launch2.2motion_detector.py3.rqt_image_view

一、安裝ROS-OpenCV

安裝OpenCVsudoapt-getinstallros-kinetic-vision-opencvlibopencv-devpython-opencv

ROS進行圖像處理是依賴于OpenCV庫的。ROS通過一個叫CvBridge的功能包，將獲取的圖像數(shù)據(jù)轉(zhuǎn)換成OpenCV的格式，OpenCV處理之后，傳回給ROS進行圖像顯示（應用），如下圖：

二、簡單案例分析

我們使用ROS驅(qū)動獲取攝像頭數(shù)據(jù)，將ROS獲得的數(shù)據(jù)通過CvBridge轉(zhuǎn)換成OpenCV需要的格式，調(diào)用OpenCV的算法庫對這個圖片進行處理（如畫一個圓），然后返回給ROS進行rviz顯示。

1.usb_cam.launch

首先我們建立一個launch文件，可以調(diào)用攝像頭驅(qū)動獲取圖像數(shù)據(jù)。運行l(wèi)aunch文件roslaunchxxx(功能包名)usb_cam.launch

launch

nodename="usb_cam"pkg="usb_cam"type="usb_cam_node"output="screen"

paramname="video_device"value="/dev/video0"/

paramname="image_width"value="1280"/

paramname="image_height"value="720"/

paramname="pixel_format"value="yuyv"/

paramname="camera_frame_id"value="usb_cam"/

paramname="io_method"value="mmap"/

/node

/launch

2.cv_bridge_test.py

建立一個py文件，是python2的。實現(xiàn)接收ROS發(fā)的圖像信息，在圖像上畫一個圓后，返回給ROS。返回的話題名稱是cv_bridge_image。運行py文件rosrunxxx(功能包名)cv_bridge_test.py

如果出現(xiàn)權(quán)限不夠的情況，記得切換到py文件目錄下執(zhí)行：sudochmod+x*.py

#!/usr/bin/envpython

#-*-coding:utf-8-*-

importrospy

importcv2

fromcv_bridgeimportCvBridge,CvBridgeError

fromsensor_msgs.msgimportImage

classimage_converter:

def__init__(self):

#創(chuàng)建cv_bridge，聲明圖像的發(fā)布者和訂閱者

self.image_pub=rospy.Publisher("cv_bridge_image",Image,queue_size=1)

self.bridge=CvBridge()

self.image_sub=rospy.Subscriber("/usb_cam/image_raw",Image,self.callback)

defcallback(self,data):

#使用cv_bridge將ROS的圖像數(shù)據(jù)轉(zhuǎn)換成OpenCV的圖像格式

try:

cv_image=self.bridge.imgmsg_to_cv2(data,"bgr8")

exceptCvBridgeErrorase:

printe

#在opencv的顯示窗口中繪制一個圓，作為標記

(rows,cols,channels)=cv_image.shape

ifcols60androws60:

cv2.circle(cv_image,(60,60),30,(0,0,255),-1)

#顯示Opencv格式的圖像

cv2.imshow("Imagewindow",cv_image)

cv2.waitKey(3)

#再將opencv格式額數(shù)據(jù)轉(zhuǎn)換成rosimage格式的數(shù)據(jù)發(fā)布

try:

self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image,"bgr8"))

exceptCvBridgeErrorase:

printe

if__name__=='__main__':

try:

#初始化ros節(jié)點

rospy.init_node("cv_bridge_test")

rospy.loginfo("Startingcv_bridge_testnode")

image_converter()

rospy.spin()

exceptKeyboardInterrupt:

print"Shuttingdowncv_bridge_testnode."

cv2.destroyAllWindows()

3.rqt_image_view

在終端下執(zhí)行rqt_image_view，訂閱cv_bridge_image話題，可以發(fā)現(xiàn)OpenCV處理之后的圖像在ROS中顯示出來。

三、CvBridge相關(guān)API

1.imgmsg_to_cv2()

將ROS圖像消息轉(zhuǎn)換成OpenCV圖像數(shù)據(jù)；

#使用cv_bridge將ROS的圖像數(shù)據(jù)轉(zhuǎn)換成OpenCV的圖像格式

cv_image=self.bridge.imgmsg_to_cv2(data,"bgr8")

exceptCvBridgeErrorase:

printe

2.cv2_to_imgmsg()

將OpenCV格式的圖像數(shù)據(jù)轉(zhuǎn)換成ROS圖像消息；

#再將opencv格式額數(shù)據(jù)轉(zhuǎn)換成rosimage格式的數(shù)據(jù)發(fā)布

self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image,"bgr8"))

exceptCvBridgeErrorase:

printe

四、利用ROS+OpenCV實現(xiàn)人臉檢測案例

1.usb_cam.launch

這個launch和上一個案例一樣先打開攝像頭驅(qū)動獲取圖像數(shù)據(jù)。運行l(wèi)aunch文件roslaunchxxx(功能包名)usb_cam.launch

launch

nodename="usb_cam"pkg="usb_cam"type="usb_cam_node"output="screen"

paramname="video_device"value="/dev/video0"/

paramname="image_width"value="1280"/

paramname="image_height"value="720"/

paramname="pixel_format"value="yuyv"/

paramname="camera_frame_id"value="usb_cam"/

paramname="io_method"value="mmap"/

/node

/launch

2.face_detector.launch

人臉檢測算法采用基于Harr特征的級聯(lián)分類器對象檢測算法，檢測效果并不佳。但是這里只是為了演示如何使用ROS和OpenCV進行圖像處理，所以不必在乎算法本身效果。整個launch調(diào)用了一個py文件和兩個xml文件，分別如下：

2.1launch

launch

nodepkg="robot_vision"name="face_detector"type="face_detector.py"output="screen"

remapfrom="input_rgb_image"to="/usb_cam/image_raw"/

rosparam

haar_scaleFactor:1.2

haar_minNeighbors:2

haar_minSize:40

haar_maxSize:60

/rosparam

paramname="cascade_1"value="$(findrobot_vision)/data/haar_detectors/haarcascade_frontalface_alt.xml"/

paramname="cascade_2"value="$(findrobot_vision)/data/haar_detectors/haarcascade_profileface.xml"/

/node

/launch

2.2face_detector.py

#!/usr/bin/envpython

#-*-coding:utf-8-*-

importrospy

importcv2

importnumpyasnp

fromsensor_msgs.msgimportImage,RegionOfInterest

fromcv_bridgeimportCvBridge,CvBridgeError

classfaceDetector:

def__init__(self):

rospy.on_shutdown(self.cleanup);

#創(chuàng)建cv_bridge

self.bridge=CvBridge()

self.image_pub=rospy.Publisher("cv_bridge_image",Image,queue_size=1)

#獲取haar特征的級聯(lián)表的XML文件，文件路徑在launch文件中傳入

cascade_1=rospy.get_param("~cascade_1","")

cascade_2=rospy.get_param("~cascade_2","")

#使用級聯(lián)表初始化haar特征檢測器

self.cascade_1=cv2.CascadeClassifier(cascade_1)

self.cascade_2=cv2.CascadeClassifier(cascade_2)

#設(shè)置級聯(lián)表的參數(shù)，優(yōu)化人臉識別，可以在launch文件中重新配置

self.haar_scaleFactor=rospy.get_param("~haar_scaleFactor",1.2)

self.haar_minNeighbors=rospy.get_param("~haar_minNeighbors",2)

self.haar_minSize=rospy.get_param("~haar_minSize",40)

self.haar_maxSize=rospy.get_param("~haar_maxSize",60)

self.color=(50,255,50)

#初始化訂閱rgb格式圖像數(shù)據(jù)的訂閱者，此處圖像topic的話題名可以在launch文件中重映射

self.image_sub=rospy.Subscriber("input_rgb_image",Image,self.image_callback,queue_size=1)

defimage_callback(self,data):

#使用cv_bridge將ROS的圖像數(shù)據(jù)轉(zhuǎn)換成OpenCV的圖像格式

try:

cv_image=self.bridge.imgmsg_to_cv2(data,"bgr8")

frame=np.array(cv_image,dtype=np.uint8)

exceptCvBridgeError,e:

printe

#創(chuàng)建灰度圖像

grey_image=cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)

#創(chuàng)建平衡直方圖，減少光線影響

grey_image=cv2.equalizeHist(grey_image)

#嘗試檢測人臉

faces_result=self.detect_face(grey_image)

#在opencv的窗口中框出所有人臉區(qū)域

iflen(faces_result)0:

forfaceinfaces_result:

x,y,w,h=face

cv2.rectangle(cv_image,(x,y),(x+w,y+h),self.color,2)

#將識別后的圖像轉(zhuǎn)換成ROS消息并發(fā)布

self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image,"bgr8"))

defdetect_face(self,input_image):

#首先匹配正面人臉的模型

ifself.cascade_1:

faces=self.cascade_1.detectMultiScale(input_image,

self.haar_scaleFactor,

self.haar_minNeighbors,

cv2.CASCADE_SCALE_IMAGE,

(self.haar_minSize,self.haar_maxSize))

#如果正面人臉匹配失敗，那么就嘗試匹配側(cè)面人臉的模型

iflen(faces)==0andself.cascade_2:

faces=self.cascade_2.detectMultiScale(input_image,

self.haar_scaleFactor,

self.haar_minNeighbors,

cv2.CASCADE_SCALE_IMAGE,

(self.haar_minSize,self.haar_maxSize))

returnfaces

defcleanup(self):

print"Shuttingdownvisionnode."

cv2.destroyAllWindows()

if__name__=='__main__':

try:

#初始化ros節(jié)點

rospy.init_node("face_detector")

faceDetector()

rospy.loginfo("Facedetectorisstarted..")

rospy.loginfo("PleasesubscribetheROSimage.")

rospy.spin()

exceptKeyboardInterrupt:

print"Shuttingdownfacedetectornode."

cv2.destroyAllWindows()

2.3兩個xml文件

鏈接

3.rqt_image_view

運行完上述兩個launch文件后，在終端下執(zhí)行rqt_image_view，訂閱cv_bridge_image話題，可以發(fā)現(xiàn)OpenCV處理之后的圖像在ROS中顯示出來。

五、利用ROS+OpenCV實現(xiàn)幀差法物體追蹤

1.usb_cam.launch

這個launch和前兩個案例一樣先打開攝像頭驅(qū)動獲取圖像數(shù)據(jù)。運行l(wèi)aunch文件roslaunchxxx(功能包名)usb_cam.launch

launch

nodename="usb_cam"pkg="usb_cam"type="usb_cam_node"output="screen"

paramname="video_device"value="/dev/video0"/

paramname="image_width"value="1280"/

paramname="image_height"value="720"/

paramname="pixel_format"value="yuyv"/

paramname="camera_frame_id"value="usb_cam"/

paramname="io_method"value="mmap"/

/node

/launch

2.motion_detector.launch

物體追蹤方法采用幀差法，追蹤效果并不佳。但是這里只是為了演示如何使用ROS和OpenCV進行圖像處理，所以不必在乎算法本身效果。整個launch調(diào)用了一個py文件，如下：

2.1launch

launch

nodepkg="robot_vision"name="motion_detector"type="motion_detector.py"output="screen"

remapfrom="input_rgb_image"to="/usb_cam/image_raw"/

rosparam

minArea:500

threshold:25

/rosparam

/node

/launch

2.2motion_detector.py

#!/usr/bin/envpython

#-*-coding:utf-8-*-

importrospy

importcv2

importnumpyasnp

fromsensor_msgs.msgimportImage,RegionOfInterest

fromcv_bridgeimportCvBridge,CvBridgeError

classmotionDetector:

def__init__(self):

rospy.on_shutdown(self.cleanup);

#創(chuàng)建cv_bridge

self.bridge=CvBridge()

self.image_pub=rospy.Publisher("cv_bridge_image",Image,queue_size=1)

#設(shè)置參數(shù)：最小區(qū)域、閾值

self.minArea=rospy.get_param("~minArea",500)

self.threshold=rospy.get_param("~threshold",25)

self.firstFrame=None

self.text="Unoccupied"

#初始化訂閱rgb格式圖像數(shù)據(jù)的訂閱者，此處圖像topic的話題名可以在launch文件中重映射

self.image_sub=rospy.Subscriber("input_rgb_image",Image,self.image_callback,queue_size=1)

defimage_callback(self,data):

#使用cv_bridge將ROS的圖像數(shù)據(jù)轉(zhuǎn)換成OpenCV的圖像格式

try:

cv_image=self.bridge.imgmsg_to_cv2(data,"bgr8")

frame=np.array(cv_image,dtype=np.uint8)

exceptCvBridgeError,e:

printe

#創(chuàng)建灰度圖像

gray=cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)

gray=cv2.GaussianBlur(gray,(21,21),0)

#使用兩幀圖像做比較，檢測移動物體的區(qū)域

ifself.firstFrameisNone:

self.firstFrame=gray

return

frameDelta=cv2.absdiff(self.firstFrame,gray)

thresh=cv2.threshold(frameDelta,self.threshold,255,cv2.THRESH_BINARY)[1]

thresh=cv2.dilate(thresh,None,iterations=2)

binary,cnts,hierarchy=cv2.findContours(thresh.copy(),cv2.RETR_EXTERNAL,cv2.C