如何解决OpenCV 内存和 CPU 使用率
我正在为我的学士项目制造 USV（无人水面车辆）。它需要保持的轨道是通过在轨道的左侧/右侧和障碍物上放置彩色浮标来制作的。 所以我需要跟踪这些物体的深度，并将所有这些信息提供给我的导航程序。
我使用 ROS 和 OpenCV 编写了一个 Python 代码，用 ZED2 相机跟踪这些浮标。但是我遇到了 cpu 和内存问题。 ubuntu 桌面开始滞后的地方。 使用 Nvidia Jetson Xavier NX，我使用了 85% 的 cpu 和 5.5+/7.59Gb 内存。
任何有兴趣查看我的代码并看看我是否在做一些愚蠢的事情的人。这可以解释我的问题。
from __future__ import print_function
import roslib
import sys
import rospy
import cv2
from main.msg import VarRed,VarGreen,varyellow,RedHSV,GreenHSV,YellowHSV,MidPoint
from sensor_msgs.msg import Image
from cv_bridge import CvBridge,CvBridgeError
import numpy as np
import imutils
import time
from collections import deque
import math
class image_converter:
def __init__(self):
self.image_subd = rospy.Subscriber("/zed2/zed_node/depth/depth_registered",Image,self.callbackDepth)
self.image_sub = rospy.Subscriber("/zed2/zed_node/rgb_raw/image_raw_color",self.callbackVideo)
self.image_pub = rospy.Publisher("/Tracking/RG_image",queue_size = 1)
self.RedHSV_sub = rospy.Subscriber("/Tracking/Red_HSV",self.redHSV)
self.GreenHSV_sub = rospy.Subscriber("/Tracking/Green_HSV",self.greenHSV)
self.YellowHSV_sub = rospy.Subscriber("/Tracking/Yellow_HSV",self.yellowHSV)
self.MidPoint_pub = rospy.Publisher("/Tracking/MidPoint",MidPoint,queue_size = 1)
self.red_bridge = CvBridge()
self.red_publisher = rospy.Publisher("/Tracking/red",VarRed,queue_size = 1)
self.green_bridge = CvBridge()
self.green_publisher = rospy.Publisher("/Tracking/green",queue_size = 1)
self.yellow_bridge = CvBridge()
self.yellow_publisher = rospy.Publisher("/Tracking/yellow",queue_size = 1)
self.RedLower = (0,101,68) # Declaring the red-specter
self.RedUpper = (15,255,255)
self.GreenLower = (75,145,48) # Declaring the green-specter
self.GreenUpper = (96,75)
self.YellowLower = (28,56,91) # Declaring the yellow-specter
self.YellowUpper = (51,152,150)
self.red_pts = deque(maxlen=14)
self.currentDepthImg=0
self.red_counter = 0
self.red_x = 0
self.red_y = 0
self.red_radius = 30
self.green_pts = deque(maxlen=14)
self.green_currentDepthImg=0
self.green_counter = 0
self.green_x = 0
self.green_y = 0
self.green_radius = 30
self.yellow_pts = deque(maxlen=14)
self.yellow_currentDepthImg=0
self.yellow_counter = 0
self.yellow_x = 0
self.yellow_y = 0
self.yellow_radius = 30
def redHSV(self,msg):
self.RedLower = (msg.r_h_low-10,msg.r_s_low-10,msg.r_v_low-10)
self.RedUpper = (msg.r_h_high+10,msg.r_s_high+10,msg.r_v_high+10)
def greenHSV(self,msg):
self.GreenLower = (msg.g_h_low-10,msg.g_s_low-10,msg.g_v_low-10)
self.GreenUpper = (msg.g_h_high+10,msg.g_s_high+10,msg.g_v_high+10)
def yellowHSV(self,msg):
self.YellowLower = (msg.y_h_low-10,msg.y_s_low-10,msg.y_v_low-10)
self.YellowUpper = (msg.y_h_high+10,msg.y_s_high+10,msg.y_v_high+10)
def callbackDepth(self,msg_depth):
try:
cv_image_depth = self.red_bridge.imgmsg_to_cv2(msg_depth,"32FC1") # CV_Bridge Depth
except CvBridgeError as e:
print(e)
return
self.currentDepthImg=cv_image_depth
try:
a=1
except CvBridgeError as e:
print(e)
return
def callbackVideo(self,data):
try:
cv_image = self.red_bridge.imgmsg_to_cv2(data,"bgr8") # CV_Bridge Video
except CvBridgeError as e:
print(e)
return
(rows,cols,channels) = cv_image.shape
frame = cv_image
blurred = cv2.GaussianBlur(frame,(21,21),0) # resize the frame,blur it,and convert it to the HSV (11,11),0
hsv = cv2.cvtColor(blurred,cv2.COLOR_BGR2HSV) # color space.
red_mask = cv2.inRange(hsv,self.RedLower,self.RedUpper) # Construct a mask for the color "red",then perform
red_mask = cv2.erode(red_mask,None,iterations=2) # a series of dilations and erosions to remove any small
red_mask = cv2.dilate(red_mask,iterations=2) # blobs thats left in the mask.
green_mask = cv2.inRange(hsv,self.GreenLower,self.GreenUpper) # construct a mask for the color "green",then perform
green_mask = cv2.erode(green_mask,iterations=2) # a series of dilations and erosions to remove any small
green_mask = cv2.dilate(green_mask,iterations=2) # blobs thats left in the mask.
yellow_mask = cv2.inRange(hsv,self.YellowLower,self.YellowUpper) # construct a mask for the color "yellow",then perform
yellow_mask = cv2.erode(yellow_mask,iterations=2) # a series of dilations and erosions to remove any small
yellow_mask = cv2.dilate(yellow_mask,iterations=2) # blobs thats left in the mask.
red_cnts = cv2.findContours(red_mask.copy(),cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE) # find contours in the mask and initialize the current
red_cnts = imutils.grab_contours(red_cnts)
red_center = None
self.red_radius = 0
green_cnts = cv2.findContours(green_mask.copy(),cv2.CHAIN_APPROX_SIMPLE) # find contours in the mask and initialize the current
green_cnts = imutils.grab_contours(green_cnts)
green_center = None
self.green_radius = 0
yellow_cnts = cv2.findContours(yellow_mask.copy(),cv2.CHAIN_APPROX_SIMPLE) # find contours in the mask and initialize the current
yellow_cnts = imutils.grab_contours(yellow_cnts)
yellow_center = None
self.yellow_radius = 0
cv_imaged=self.currentDepthImg
#-----------------------------------------RED_START------------------------------------------------------
if len(red_cnts) > 0: # only proceed if at least one contour was found
red_c = max(red_cnts,key=cv2.contourArea) # find the largest contour in the red_mask,then use
((self.red_x,self.red_y),self.red_radius) = cv2.minenclosingCircle(red_c) # it to compute the minimum enclosing circle and
red_M = cv2.moments(red_c) # centroid
red_center = (int(red_M["m10"] / red_M["m00"]),int(red_M["m01"] / red_M["m00"]))
if self.red_radius > 5: # only proceed if the radius meets a minimum size
cv2.circle(frame,(int(self.red_x),int(self.red_y)),int(self.red_radius),(0,255),2) # draw the circle and centroid on the red_frame,cv2.circle(frame,red_center,5,-1) # then update the list of tracked points
msg = VarRed()
msg.r_visible = True
#if self.red_y == self.red_y and self.red_x == self.red_x:
r_length = cv_imaged[int(self.red_y),int(self.red_x)] # length to object
msg.r_x = self.red_x
msg.r_y = self.red_y
msg.r_rad = self.red_radius
ToRad = 2*np.pi/360 # = 0.01745329252
ToDeg = 360/(2*np.pi) # = 57.29577951308
# Printing pixel values
cv2.rectangle(frame,0),(200,190),-1)
cv2.putText(frame,str("L: %.3f" %r_length),((int(self.red_x)),cv2.FONT_HERShey_COMPLEX,1,2)
cv2.putText(frame,str("RX: %.1f" %msg.r_x +" px"),(10,30),0.8,(255,1)
cv2.putText(frame,str("RY: %.1f" %msg.r_y + " px"),60),1)
# For X-direction
red_l_cm = (r_length*100) # Converting to Centimeters
start_x_r = 960/(math.tan((55*ToRad))) # finding start x-length in px
ang_x_r = (math.atan((self.red_x-960)/start_x_r))*ToDeg # finding horizontal angle
red_x_cm = (red_l_cm*math.sin((ang_x_r)*ToRad))
cv2.putText(frame,str("RXC: %.1f" %red_x_cm + " cm"),90),str("X Ang: %.1f" %ang_x_r),150),1)
# For Y-direction
start_y_r = 540/(math.tan((35*ToRad))) # finding start y-length in px
ang_y_r = ((math.atan((self.red_y-540)/start_y_r))*ToDeg)*-1 # finding vertical angle
red_y_cm = (red_l_cm/math.tan((ang_y_r*ToRad)+(math.pi/2)))*-1 # finding the y-length
cv2.putText(frame,str("RYC: %.1f" %red_y_cm + " cm"),120),str("Y Ang: %.1f" %ang_y_r),180),1)
red_z = (math.cos(abs(ang_x_r)*ToRad))*red_l_cm
self.red_pts.appendleft(red_center)
msg.r_length = red_l_cm
msg.r_xc = red_x_cm
msg.r_yc = red_y_cm
msg.r_angle = ang_x_r # update the points queue
msg.r_z = red_z
self.red_publisher.publish(msg)
for i in range(1,len(self.red_pts)): # loop over the set of points
if self.red_pts[i - 1] is None or self.red_pts[i] is None: # if either of the tracked points
continue # are None,ignore them.
thickness = int(np.sqrt(64 / float(i + 1)) * 2.5) # otherwise,compute the thickness of the line and
cv2.line(frame,self.red_pts[i - 1],self.red_pts[i],thickness) # draw the connecting lines
if self.red_radius < 5:
msg = VarRed()
msg.r_visible = False
self.red_publisher.publish(msg)
#-----------------------------------------RED_END------------------------------------------------------
#-----------------------------------------GREEN_START------------------------------------------------------
if len(green_cnts) > 0: # same as in red,but for green
green_c = max(green_cnts,key=cv2.contourArea)
((self.green_x,self.green_y),self.green_radius) = cv2.minenclosingCircle(green_c)
green_M = cv2.moments(green_c)
green_center = (int(green_M["m10"] / green_M["m00"]),int(green_M["m01"] / green_M["m00"]))
if self.green_radius > 5:
cv2.circle(frame,(int(self.green_x),int(self.green_y)),int(self.green_radius),2)
cv2.circle(frame,green_center,-1)
ToRad = 2*np.pi/360 # = 0.01745329252
ToDeg = 360/(2*np.pi) # = 57.29577951308
msg1 = VarGreen()
msg1.g_visible = True
g_length = cv_imaged[int(self.green_y),int(self.green_x)]
msg1.g_x = self.green_x
msg1.g_y = self.green_y
msg1.g_rad = self.green_radius
# Printing pixel values
cv2.rectangle(frame,(1740,(1920,200),str("L: %.3f" %g_length),((int(self.green_x)),str("GX: %.1f" %msg1.g_x +"px"),str("GY: %.1f" %msg1.g_y + "px"),1)
# For X-direction
green_l_cm = (g_length*100)
start_x_g = 960/(math.tan((55*2*np.pi/360)))
ang_x_g = (math.atan((self.green_x-960)/start_x_g))*57.295779513
green_x_cm = (green_l_cm*math.sin((ang_x_g)*ToRad))
# For Y-direction
start_y_g = 540/(math.tan((35*2*np.pi/360)))
ang_y_g = ((math.atan((self.green_y-540)/start_y_g))*57.295779513)*-1
green_y_cm = green_l_cm/math.tan(ang_y_g*ToRad+(math.pi/2))*-1
cv2.putText(frame,str("GXC: %.1f" %green_x_cm + "cm"),str("X Ang: %.1f" %ang_x_g),str("GYC: %.1f" %green_y_cm + "cm"),str("Y Ang: %.1f" %ang_y_g),1)
green_z = (math.cos(abs(ang_x_g)*ToRad))*green_l_cm
self.green_pts.appendleft(green_center)
msg1.g_length = green_l_cm
msg1.g_xc = green_x_cm
msg1.g_yc = green_y_cm
msg1.g_angle = ang_x_g
msg1.g_z = green_z
self.green_publisher.publish(msg1)
for i in range(1,len(self.green_pts)):
if self.green_pts[i - 1] is None or self.green_pts[i] is None:
continue
thickness = int(np.sqrt(64 / float(i + 1)) * 2.5)
cv2.line(frame,self.green_pts[i - 1],self.green_pts[i],thickness)
if self.green_radius < 5:
msg1 = VarGreen()
msg1.g_visible = False
self.green_publisher.publish(msg1)
#-----------------------------------------GREEN_END------------------------------------------------------
#-----------------------------------------YELLOW_START------------------------------------------------------
if len(yellow_cnts) > 0: # only proceed if at least one contour was found
yellow_c = max(yellow_cnts,key=cv2.contourArea) # find the largest contour in the yellow_mask,then use
((self.yellow_x,self.yellow_y),self.yellow_radius) = cv2.minenclosingCircle(yellow_c) # it to compute the minimum enclosing circle and
yellow_M = cv2.moments(yellow_c) # centroid
yellow_center = (int(yellow_M["m10"] / yellow_M["m00"]),int(yellow_M["m01"] / yellow_M["m00"]))
if self.yellow_radius > 5: # only proceed if the radius meets a minimum size
cv2.circle(frame,(int(self.yellow_x),int(self.yellow_y)),int(self.yellow_radius),2) # draw the circle and centroid on the yellow_frame,yellow_center,-1) # then update the list of tracked points
ToRad = 2*np.pi/360 # = 0.01745329252
ToDeg = 360/(2*np.pi) # = 57.29577951308
msg2 = varyellow()
msg2.y_visible = True
y_length = cv_imaged[int(self.yellow_y),int(self.yellow_x)] # length to object
msg2.y_x = self.yellow_x
msg2.y_y = self.yellow_y
msg2.y_rad = self.yellow_radius
cv2.putText(frame,str("L: %.3f" %y_length),((int(self.yellow_x)),2)
# For X-direction
yellow_l_cm = y_length*100 # Converting to Centimeters
start_x_y = 960/(math.tan((55*2*np.pi/360))) # finding start x-length in px
ang_x_y = (math.atan((self.yellow_x-960)/start_x_y))*57.295779513 # finding horizontal angle
#yellow_x = yellow_l_cm/math.tan((ang_x_y/57.295779513)) # finding the x-length
yellow_x_cm = (yellow_l_cm*math.sin((ang_x_y)*ToRad))
# For Y-direction
start_y_y = 540/(math.tan((35*2*np.pi/360))) # finding start y-length in px
ang_y_y = ((math.atan((self.yellow_y-540)/start_y_y))*57.295779513)*-1 # finding vertical angle
#yellow_y = yellow_l_cm/math.tan((ang_y_y/57.295779513)) # finding the y-length
yellow_y_cm = yellow_l_cm/math.tan(ang_y_y*ToRad+(math.pi/2))*-1
yellow_z = (math.cos(abs(ang_x_y)*ToRad))*yellow_l_cm
self.yellow_pts.appendleft(yellow_center)
msg2.y_length = yellow_l_cm
msg2.y_xc = yellow_x_cm
msg2.y_yc = yellow_y_cm
msg2.y_angle = ang_x_y # update the points queue
msg2.y_z = yellow_z
self.yellow_publisher.publish(msg2)
for i in range(1,len(self.yellow_pts)): # loop over the set of points
if self.yellow_pts[i - 1] is None or self.yellow_pts[i] is None: # if either of the tracked points
continue # are None,ignore them.
thickness = int(np.sqrt(64 / float(i + 1)) * 2.5) # otherwise,self.yellow_pts[i - 1],self.yellow_pts[i],thickness) # draw the connecting lines
if self.yellow_radius < 5:
msg2 = varyellow()
msg2.y_visible = False
self.yellow_publisher.publish(msg2)
#-----------------------------------------YELLOW_END------------------------------------------------------
try:
if (self.green_radius > 5) & (self.red_radius > 5): # if you can see both colors,proceed
ToRad = 2*np.pi/360 # = 0.01745329252
ToDeg = 360/(2*np.pi) # = 57.29577951308
red_z = (math.cos(abs(ang_x_r)*ToRad))*red_l_cm
green_z = (math.cos(abs(ang_x_g)*ToRad))*green_l_cm
Delta_z = abs(red_z-green_z)
Tot_x = abs(green_x_cm) + abs(red_x_cm)
if Delta_z == Delta_z and Tot_x == Tot_x:
red_green_angle = (math.atan(Delta_z/Tot_x))*ToDeg
normal_angle = red_green_angle
if green_l_cm >= red_l_cm:
normal_angle = red_green_angle*-1
if green_l_cm < red_l_cm:
normal_angle = red_green_angle
MidPoint_data = MidPoint()
MidPoint_data.angle = normal_angle
self.MidPoint_pub.publish(MidPoint_data)
length_between_x = math.sqrt((Tot_x*Tot_x)+(Delta_z*Delta_z))
Delta_y = abs(red_y_cm-green_y_cm)
length_between = math.sqrt((length_between_x*length_between_x)+(Delta_y*Delta_y))
#dx = green_x_cm - red_x_cm # Finding the space between the colors in x-direction
#dy = green_y_cm - red_y_cm # Finding the space between the colors in y-direction
# Calculating the direct length between the colors in cm
#cv2.rectangle(frame,(500,(680,160),-1)
#cv2.putText(frame,str("dist: %.1f" %length_between + " cm"),1)
#Middle_x = dx
#Middle_y = dy
MP_X = (msg1.g_x + msg.r_x)/2
MP_Y = (msg1.g_y + msg.r_y)/2
#Middle_Point_Angle = (math.atan((MP_X-960)/start_x_g))*57.295779513
#Middle_Point_Angle = ang_x_g - ang_x_r
#Middle_Point_Length =(red_x_cm-abs(Middle_x))/(math.sin((math.pi/2)-(Middle_Point_Angle*((2*math.pi)/720))))
#Middle_Point_Length =((red_x_cm-abs(Middle_x))/(math.cos(Middle_Point_Angle*((2*math.pi)/720))))
#cv2.putText(frame,str("MX: %.1f" %Middle_x + " cm"),1)
#cv2.putText(frame,str("MY: %.1f" %Middle_y + " cm"),1)
if MP_X == MP_X and MP_Y == MP_Y:
cv2.circle(frame,(int(MP_X),int(MP_Y)),8,str("M_L: %.1f" %Middle_Point_Length + " cm"),str("M_ang: %.1f" %Middle_Point_Angle),1)
cv2.line(frame,2)
#MidPoint_data = MidPoint()
#MidPoint_data.z = Middle_Point_Length
#self.MidPoint_pub.publish(MidPoint_data)
cv2.line(frame,(960,1280),1)
cv2.line(frame,540),1)
self.image_pub.publish(self.red_bridge.cv2_to_imgmsg(frame,"bgr8"))
except CvBridgeError as e:
print(e)
return
def main(args):
ic = image_converter()
rospy.init_node('Color_Tracker',anonymous=True)
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
if __name__ == '__main__':
main(sys.argv)
版权声明：本文内容由互联网用户自发贡献，该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容， 请发送邮件至 dio@foxmail.com 举报，一经查实，本站将立刻删除。
