Related
I am labeling some recordings with quite a few objects, I notice that when I do this in the naieve OpenCV manner, the video export takes ~real-time (hour long video = hour long export). I'd like to accelerate this process, and I was looking at some forums and some code examples about how to use a separate thread to read the frames and then use the main thread to do the frame processing. When I try this, however, the exported video is way too fast, and my added labels/annotations do not sync with the actual recording. Furthermore, even though I set the export frame rate the same as the input video (30fps), it seems that only my added annotations follow it. Here's my code, the multiprocessing class was adapted from (https://github.com/gilbertfrancois/video-capture-async), I took their threadlock idea, but it did not do anything for me.
class Threaded_VidCapture(object):
def __init__(self, videoSrc):
#Constructor parameters
self.VidIn = cv2.VideoCapture(videoSrc)
self.Terminate = False
self.Ret, self.Frame = self.VidIn.read()
#Video Parameters
self.FPS = int(self.VidIn.get(cv2.CAP_PROP_FPS))
self.Resolution = (
int(self.VidIn.get(cv2.CAP_PROP_FRAME_WIDTH)),
int(self.VidIn.get(cv2.CAP_PROP_FRAME_HEIGHT))
)
self.FrameCount = int(self.VidIn.get(cv2.CAP_PROP_FRAME_COUNT))
self.ThreadLock = threading.Lock()
def startProcess(self):
self.ThreadStart = Thread(target=self.retrieveFrames, args=())
self.ThreadStart.daemon = True
self.ThreadStart.start()
return(self)
def retrieveFrames(self):
while self.Terminate is False:
Ret, Frame = self.VidIn.read()
with self.ThreadLock:
self.Ret = Ret
self.Frame = Frame
def read(self):
with self.ThreadLock:
return(self.Frame.copy(), self.Ret)
def TerminateProgram(self):
self.Terminate = True
self.ThreadStart.join()
self.VidIn.release()
def annotateVideo_wrapper(RotationalCounts, Coords_FromLabel, Coords_ToLabel,
videoFile, videoExport, **kwargs):
GetFrames = Threaded_VidCapture(videoSrc=videoFile).startProcess()
Write = cv2.VideoWriter_fourcc(*"XVID")
Export = cv2.VideoWriter(videoExport, Write, GetFrames.FPS, GetFrames.Resolution)
def labelAnnotation(Frame, Text, Position, Color_3ChannelRGB, **kwargs):
ScaleKwargs = dict([(key, vals) for key, vals in kwargs.items()])
#Base parameters from CV2 website
Font = cv2.FONT_HERSHEY_PLAIN
if len(ScaleKwargs) == 0:
Scale = 2
else:
Scale = ScaleKwargs["Scale"]
Color = (0,0,0)#Black
Thickness = cv2.FILLED
Margin = 5
TextSize = cv2.getTextSize(text=str(Text), fontFace=Font, fontScale=Scale, thickness=Thickness)
EndX = Position[0] + TextSize[0][0] + Margin
EndY = Position[1] - TextSize[0][1] - Margin
cv2.rectangle(Frame, Position, (EndX, EndY), Color_3ChannelRGB, Thickness)
cv2.putText(Frame, str(Text), Position, Font, Scale, Color, 1, cv2.LINE_AA)
if kwargs:
HashMap = {keys: Val for keys, Val in kwargs.items()}
try:
Graphs = Graph.Live_movementTrace(
DataFrame=HashMap["DataFrame"],
Label=HashMap["Label"],
RotationMetaData=HashMap["RotationMetaData"],
IndexLimit=HashMap["IndexLimit"]
)
except KeyError:
raise(KeyError("Missing arguments, re-run the program with the correct input or leave this function blank"))
fig = mp.figure()
fig.suptitle("Live Movement Plot", fontweight="bold", fontsize=14)
Hash = {"CW_x":[], "CW_y":[], "CCW_x":[], "CCW_y":[]}
line1, = mp.plot(Hash["CW_x"], Hash["CW_y"], color = "blue")
line2, = mp.plot(Hash["CCW_x"], Hash["CCW_y"], color = "red")
mp.xlim(0, GetFrames.Resolution[0])
mp.ylim(0, GetFrames.Resolution[1])
ax = mp.gca()
ax.invert_yaxis()
else:
Graphs = []
Ind=0
ProgressBar = tqdm(total = len(Coords_FromLabel))
while(Ind < GetFrames.FrameCount):
Frame = GetFrames.read()[0]
####################
#Add Coordinate System
####################
cv2.line(Frame, (int(Coords_FromLabel[Ind][0]), int(Coords_FromLabel[Ind][1])), (int(Coords_FromLabel[Ind][0]), 0), (0, 0, 0), 5, 8, 0)#MinY
cv2.line(Frame, (int(Coords_FromLabel[Ind][0]), int(Coords_FromLabel[Ind][1])), (int(Coords_FromLabel[Ind][0]), GetFrames.Resolution[1]), (255, 0, 0), 5, 8, 0)#MaxY
cv2.line(Frame, (int(Coords_FromLabel[Ind][0]), int(Coords_FromLabel[Ind][1])), (0, int(Coords_FromLabel[Ind][1])), (0, 255, 0), 5, 8, 0)#MinX
cv2.line(Frame, (int(Coords_FromLabel[Ind][0]), int(Coords_FromLabel[Ind][1])), (GetFrames.Resolution[0], int(Coords_FromLabel[Ind][1])), (255, 255, 255), 5, 8, 0)#MaxX
####################
#Add Coutners
####################
labelAnnotation(Frame, f"CW: {round(RotationalCounts[0][Ind], 3)}", (20, 100), (255, 255, 255))
labelAnnotation(Frame, f"CCW: {round(RotationalCounts[1][Ind], 3)}", (20, 130), (0, 0, 255))
if Ind < 10000:
labelAnnotation(Frame, f"Frame Num: {Ind}", (20, 160), (100, 100, 100))
elif Ind >= 10000:
labelAnnotation(Frame, f"Frame Num: {Ind}", (20, 160), (100, 100, 100), Scale = 1.5)
####################
#Add Vectors
####################
cv2.line(Frame, (int(Coords_FromLabel[Ind][0]), int(Coords_FromLabel[Ind][1])), (int(Coords_ToLabel[Ind][0]), int(Coords_ToLabel[Ind][1])), (0, 0, 255), 5, 8, 0)
if len(Graphs) != 0:
if ((Ind in Graphs[3]) and (Ind != 0)):
for keys in Hash:
Hash[keys].clear()
Hash["CW_x"].append(Graphs[0][f'{HashMap["Label"]}_x'][Ind])
Hash["CW_y"].append(Graphs[0][f'{HashMap["Label"]}_y'][Ind])
line1.set_xdata(Hash["CW_x"])
line1.set_ydata(Hash["CW_y"])
Hash["CCW_x"].append(Graphs[1][Ind])
Hash["CCW_y"].append(Graphs[2][Ind])
line2.set_xdata(Hash["CCW_x"])
line2.set_ydata(Hash["CCW_y"])
fig.canvas.draw()
img = np.fromstring(fig.canvas.tostring_rgb(), dtype=np.uint8, sep='')
img = img.reshape(fig.canvas.get_width_height()[::-1] + (3,))
img = cv2.cvtColor(img,cv2.COLOR_RGB2BGR)
#Needs to be a variable offset
x_offset = GetFrames.Resolution[0] - 450
y_offset = GetFrames.Resolution[1] - 340
# print(Frame.shape)
x_end = x_offset + img.shape[1]
y_end = y_offset + img.shape[0]
Frame[y_offset:y_end, x_offset:x_end] = img
Export.write(Frame)
#if HashMap["ShowFrame"] is True:
#cv2.imshow("Frame", Frame)
cv2.waitKey(1) & 0xFF
# if ((cv2.waitKey(1) & 0xFF == ord("q")) or (GetFrames.Terminate is True)):
# GetFrames.TerminateProgram()
# break
ProgressBar.update(1)
Ind += 1
Export.release()
cv2.destroyAllWindows()
GetFrames.TerminateProgram()
There's a lot that I'm adding to each frame, and again this works fine in just the naive approach, I get about 23 it/s as measured with tqdm, which is close to FPS mark. I'd like to be above 30 it/s, but I don't know if that's possible.
I have been following some tutorials online on how to make a hand tracking program which can control the volume of my PC via OpenCV. I am on a macbook pro with the M1 chip, using Python 3.8.2, and MacOS Big Sur 11.6.
Whenever I run my program, I get 15 FPS instead of the usual 30 that most people get when following the tutorial. Further investigation showed that only 1 line of code is causing my fps to drop from 30 to 15.
self.results = self.hands.process(imgRGB)
Does anyone know any way to improve my FPS, and why this line causes such a massive drop in FPS? Full code is below
First part here is the hand detection script, where the problematic line of code is (under FindHands function):
import cv2
import mediapipe as mp
import time
class handDetector():
def __init__(self, mode=False, maxHands=2, modelComplexity=1, detectionCon=0.8, trackCon=0.8):
self.mode = mode
self.maxHands = maxHands
self.modelComplex = modelComplexity
self.detectionCon = detectionCon
self.trackCon = trackCon
self.mpHands = mp.solutions.hands
self.hands = self.mpHands.Hands(self.mode, self.maxHands, self.modelComplex, self.detectionCon,
self.trackCon)
self.mpDraw = mp.solutions.drawing_utils
def findHands(self,img, draw = True):
imgRGB = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
self.results = self.hands.process(imgRGB)
# print(results.multi_hand_landmarks)
if self.results.multi_hand_landmarks:
for handLms in self.results.multi_hand_landmarks:
if draw:
self.mpDraw.draw_landmarks(img, handLms, self.mpHands.HAND_CONNECTIONS)
return img
def findPosition(self, img, handNo = 0, draw = True):
lmlist = []
if self.results.multi_hand_landmarks:
myHand = self.results.multi_hand_landmarks[handNo]
for id, lm in enumerate(myHand.landmark):
h, w, c = img.shape
cx, cy = int(lm.x * w), int(lm.y * h)
lmlist.append([id, cx, cy])
if draw:
cv2.circle(img, (cx, cy), 7, (255, 0, 255), cv2.FILLED)
return lmlist
def main():
pTime = 0
cTime = 0
cap = cv2.VideoCapture(0)
detector = handDetector()
while True:
success, img = cap.read()
img = detector.findHands(img)
lmlist = detector.findPosition(img)
if len(lmlist) != 0:
print(lmlist[4])
cTime = time.time()
fps = 1 / (cTime - pTime)
pTime = cTime
cv2.putText(img, str(int(fps)), (10, 70), cv2.FONT_HERSHEY_PLAIN, 3, (255, 0, 255), 3)
cv2.imshow("Image", img)
cv2.waitKey(1)
if __name__ == "__main__":
main()
This next part is the script that controls the volume control, which on another note further reduces my FPS to 5 when a hand is in frame, which is apparently due to Osascript not being very good:
import cv2
import time
import numpy as np
import DoneHand as dh
import math
import osascript
minVol = 0
maxVol = 100
wCam, hCam = 640, 480
pTime = 0
cap = cv2.VideoCapture(0)
detector = dh.handDetector()
while True:
success, img = cap.read()
img = detector.findHands(img)
lmList = detector.findPosition(img, draw = False)
if len(lmList) != 0:
#print(lmList[4], lmList[8])
x1, y1 = lmList[4][1], lmList[4][2]
x2, y2 = lmList[8][1], lmList[8][2]
cx, cy = (x1 + x2) // 2, (y1 + y2) // 2
cv2.circle(img, (x1, y1), 15, (255, 0, 255), cv2.FILLED)
cv2.circle(img, (x2, y2), 15, (255, 0, 255), cv2.FILLED)
cv2.circle(img, (cx, cy), 15, (255, 0, 255), cv2.FILLED)
cv2.line(img, (x1, y1), (x2, y2), (255, 0, 255), 3)
length = math.hypot(x2 - x1, y2 - y1)
print(length)
#Hand range 55 - 350
#Volume range 0 - 100
vol = np.interp(length, [55, 350], [minVol, maxVol])
print(vol)
target_volume = vol
#osascript.osascript("set volume output volume {}".format(target_volume))
if length < 55:
cv2.circle(img, (cx, cy), 15, (0, 255, 0), cv2.FILLED)
cTime = time.time()
fps = 1/(cTime-pTime)
pTime = cTime
cv2.putText(img,str(int(fps)), (10,70), cv2.FONT_HERSHEY_PLAIN, 3, (255,0,255), 3)
cv2.imshow("Img", img)
cv2.waitKey(1)
Thank you in advance!
I have been working on these lines, and I want to create it like in picture, but I got complicated. How do I get line symmetrical like in picture below?
This is what I tried so far, not much :/
from PIL import Image, ImageDraw
img = Image.new('RGB', (1000, 1000), (255, 255, 255))
draw = ImageDraw.Draw(img)
for y in range (-4000, 2000, 200):
draw.line(((y, img.size[0]+img.size[0]),
(img.size[1]+img.size[1]+img.size[1], y)), (0, 0, 0), 20)
for x in range (-2000, 1000, 100):
draw.line(((x,-x),(x+img.size[0], -x+img.size[1]+img.size[1])), (0, 0, 0), 20)
I want to create lines like this...
I think you created too complicated calculations and this made problem
Symmetrical lines should have similar calculations but with different sign.
Both should have y from 0 to height
from PIL import Image, ImageDraw
img = Image.new('RGB', (1000, 1000), (255, 255, 255))
draw = ImageDraw.Draw(img)
w = img.size[0]
h = img.size[1]
offset = 400
step = 100
for x in range(-offset, w+offset, step):
draw.line(((x, 0), (x+offset, h)), (0, 0, 0), 20)
draw.line(((x, 0), (x-offset, h)), (0, 0, 0), 20)
img.show()
Gives
And for offset = image_width (offset = 1000) it gives rectangles
EDIT:
Version which use move_x, move_y to move lines.
Because after moving lines it shows end of lines so I use line_width to start and end line outside window.
from PIL import Image, ImageDraw
img = Image.new('RGB', (1000, 1000), (0, 200, 0))
draw = ImageDraw.Draw(img)
w = img.size[0]
h = img.size[1]
line_width = 10 #
move_x = 10
move_y = 50
offset = w # 400
step = 200
for x in range(-offset, w+offset, step):
x1 = x + move_x - move_y - line_width
x2 = x1 + offset + line_width*2
x3 = x + move_x + move_y + line_width
x4 = x3 - offset - line_width*2
draw.line(((x1, 0-line_width), (x2, h+line_width)), (0, 0, 0), line_width)
draw.line(((x3, 0-line_width), (x4, h+line_width)), (0, 0, 0), line_width)
img.show()
img.save('output.png')
**I am trying to create a virtual mouse. I am using packages like opencv-python, mediapipe, time, numpy, And also my own package. But seems to have some error. Can you help me on it. At first I have created the Module. Then the mouse itself. But I don't understand the problem in it. **
Hand tracking module that I have created
import cv2
import mediapipe as mp
import time
class handDetector():
def __init__(self, mode=False, maxHands=2, detectionCon=0.5, trackCon=0.5):
self.mode = mode
self.maxHands = maxHands
self.detectionCon = detectionCon
self.trackCon = trackCon
self.mpHands = mp.solutions.hands
self.hands = self.mpHands.Hands(self.mode, self.maxHands,
self.detectionCon, self.trackCon)
self.mpDraw = mp.solutions.drawing_utils
def findHands(self, img, draw=True):
imgRGB = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
self.results = self.hands.process(imgRGB)
print(self.results.multi_hand_landmarks)
if self.results.multi_hand_landmarks:
for handLms in self.results.multi_hand_landmarks:
if draw:
self.mpDraw.draw_landmarks(img, handLms, self.mpHands.HAND_CONNECTIONS)
return img
def findPosition(self, img, handNo=0, draw=True):
lmList = []
if self.results.multi_hand_landmarks:
myHand = self.results.multi_hand_landmarks [handNo]
for id, lm in enumerate(myHand.landmark):
print(id, lm)
h, w, c = img.shape
cx, cy = int(lm.x * w), int(lm.y * h)
print(id, cx, cy)
lmList.append( [ id, cx, cy])
if draw:
cv2.circle(img, (cx, cy), 15, (255, 0, 255), cv2.FILLED)
return lmList
def main():
pTime = 0
cTime = 0
cap = cv2.VideoCapture(1)
detector = handDetector()
while True:
success, img = cap.read()
img = detector.findHands(img)
lmList = detector.findPosition(img)
if len(lmList) != 0:
print(lmList[4])
cTime = time.time()
fps = 1 / (cTime - pTime)
pTime = cTime
cv2.putText(img, str(int(fps)), (10, 70), cv2.FONT_HERSHEY_PLAIN, 3,
(255, 0, 255), 3)
cv2.imshow("Image", img)
if cv2.waitKey(1) == ord("q"):
break
if __name__ == "__main__":
main()
Here is the software code
import cv2
import cvzone.HandTrackingModule
import numpy as np
import HandTrackingModule as htm
import time
import autopy
##########################
wCam, hCam = 640, 480
frameR = 100 # Frame Reduction
smoothening = 7
#########################
pTime = 0
plocX, plocY = 0, 0
clocX, clocY = 0, 0
cap = cv2.VideoCapture(1)
cap.set(3, wCam)
cap.set(4, hCam)
detector = htm.handDetector()
wScr, hScr = autopy.screen.size()
# print(wScr, hScr)
while True:
# 1. Find hand Landmarks
success, img = cap.read()
img = detector.findHands(img)
lmList, bbox = detector.findPosition(img)
# 2. Get the tip of the index and middle fingers
if len(lmList) != 0:
x1, y1 = lmList [8][1:]
x2, y2 = lmList [12][1:]
# print(x1, y1, x2, y2)
# 3. Check which fingers are up
fingers = cvzone.HandTrackingModule.HandDetector.fingersUp()
# print(fingers)
cv2.rectangle(img, (frameR, frameR), (wCam - frameR, hCam - frameR),
(255, 0, 255), 2)
# 4. Only Index Finger : Moving Mode
if fingers[1] == 1 and fingers[2] == 0:
# 5. Convert Coordinates
x3 = np.interp(x1, (frameR, wCam - frameR), (0, wScr))
y3 = np.interp(y1, (frameR, hCam - frameR), (0, hScr))
# 6. Smoothen Values
clocX = plocX + (x3 - plocX) / smoothening
clocY = plocY + (y3 - plocY) / smoothening
# 7. Move Mouse
autopy.mouse.move(wScr - clocX, clocY)
cv2.circle(img, (x1, y1), 15, (255, 0, 255), cv2.FILLED)
plocX, plocY = clocX, clocY
# 8. Both Index and middle fingers are up : Clicking Mode
if fingers [1] == 1 and fingers[2] == 1:
# 9. Find distance between fingers
length, img, lineInfo = detector.findDistance(8, 12, img)
print(length)
# 10. Click mouse if distance short
if length [40]:
cv2.circle(img, (lineInfo [4], lineInfo[5]),
15, (0, 255, 0), cv2.FILLED)
autopy.mouse.click()
# 11. Frame Rate
cTime = time.time()
fps = 1 / (cTime - pTime)
pTime = cTime
cv2.putText(img, str(int(fps)), (20, 50), cv2.FONT_HERSHEY_PLAIN, 3,
(255, 0, 0), 3)
# 12. Display
cv2.imshow("Image", img)
cv2.waitKey(1)
Error:
lmList, bbox = detector.findPosition(img)
ValueError: not enough values to unpack (expected 2, got 0)
Pleas help me on this. This is the software code. I am trying to create virtual-mouse using these packages and also I created a module myself. But seems to have some error. I hope you can help. May God bless the helpers
Function detector.findPosition returns a list lmList, but when called in your main script its output gets assigned (and unpacked) to two variables, lmList, bbox. So the function needs to output two values, not just one:
def findPosition(self, img, handNo=0, draw=True):
# Do something to produce lmList and bbox...
return lmList, bbox
If you are only interested in the list output, the you can always do:
lmList, _ = detector.findPosition(img)
I'm making a USV (Unmanned Surface Vehicle) for my bachelor project. The track it needs to keep is made by having coloured buoys on the left/right side of the track and for obstacles.
So I need to track the depth of these objects and give all that information on to my navigation program.
And I have made a Python code using ROS and OpenCV to track these buoys with a ZED2 camera. But I'm having CPU and memory issues. Where the ubuntu desktop starts to lag.
Using a Nvidia Jetson Xavier NX and I’m using 85% of the CPU and 5,5+/7.59Gb Memory.
Anyone interested in looking over my code and see if I'm doing something stupid. That would explain my issues.
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",Image,self.callbackVideo)
self.image_pub = rospy.Publisher("/Tracking/RG_image", Image, queue_size = 1)
self.RedHSV_sub = rospy.Subscriber("/Tracking/Red_HSV", RedHSV, self.redHSV)
self.GreenHSV_sub = rospy.Subscriber("/Tracking/Green_HSV", GreenHSV, self.greenHSV)
self.YellowHSV_sub = rospy.Subscriber("/Tracking/Yellow_HSV", YellowHSV, 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", VarGreen, queue_size = 1)
self.yellow_bridge = CvBridge()
self.yellow_publisher = rospy.Publisher("/Tracking/yellow", VarYellow, 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, 255, 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, None, 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, None, iterations=2) # a series of dilations and erosions to remove any small
green_mask = cv2.dilate(green_mask, None, 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, None, iterations=2) # a series of dilations and erosions to remove any small
yellow_mask = cv2.dilate(yellow_mask, None, 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.RETR_EXTERNAL, 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.RETR_EXTERNAL, 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, 255), 2) # draw the circle and centroid on the red_frame,
cv2.circle(frame, red_center, 5, (0, 255, 255), -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, 0), (200, 190), (0,0,0), -1)
cv2.putText(frame, str("L: %.3f" %r_length), ((int(self.red_x)),int(self.red_y)), cv2.FONT_HERSHEY_COMPLEX, 1, (0,255,255), 2)
cv2.putText(frame, str("RX: %.1f" %msg.r_x +" px"), (10,30), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
cv2.putText(frame, str("RY: %.1f" %msg.r_y + " px"), (10,60), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 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"), (10,90), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
cv2.putText(frame, str("X Ang: %.1f" %ang_x_r), (10,150), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 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"), (10,120), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
cv2.putText(frame, str("Y Ang: %.1f" %ang_y_r), (10,180), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 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], (0, 255, 255), 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), (0, 255, 255), 2)
cv2.circle(frame, green_center, 5, (0, 255, 255), -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, 0), (1920, 200), (0,0,0), -1)
cv2.putText(frame, str("L: %.3f" %g_length), ((int(self.green_x)),int(self.green_y)), cv2.FONT_HERSHEY_COMPLEX, 1, (0,255,255), 2)
cv2.putText(frame, str("GX: %.1f" %msg1.g_x +"px"), (1740,30), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
cv2.putText(frame, str("GY: %.1f" %msg1.g_y + "px"), (1740,60), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 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"), (1740,90), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
cv2.putText(frame, str("X Ang: %.1f" %ang_x_g), (1740,150), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
cv2.putText(frame, str("GYC: %.1f" %green_y_cm + "cm"), (1740,120), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
cv2.putText(frame, str("Y Ang: %.1f" %ang_y_g), (1740,180), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 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], (0, 255, 255), 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), (0, 0, 200), 2) # draw the circle and centroid on the yellow_frame,
cv2.circle(frame, yellow_center, 5, (0, 0, 200), -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)),int(self.yellow_y)), cv2.FONT_HERSHEY_COMPLEX, 1, (0,0,200), 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, compute the thickness of the line and
cv2.line(frame, self.yellow_pts[i - 1], self.yellow_pts[i], (0, 0, 255), 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, 0), (680, 160), (0,0,0), -1)
#cv2.putText(frame, str("Dist: %.1f" %length_between + " cm"), (500,30), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 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"), (500,60), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
#cv2.putText(frame, str("MY: %.1f" %Middle_y + " cm"), (500,90), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
if MP_X == MP_X and MP_Y == MP_Y:
cv2.circle(frame, (int(MP_X), int(MP_Y)), 8, (0, 0, 255), -1)
#cv2.putText(frame, str("M_L: %.1f" %Middle_Point_Length + " cm"), (500,120), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
#cv2.putText(frame, str("M_ang: %.1f" %Middle_Point_Angle), (500,150), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
cv2.line(frame, (int(self.red_x),int(self.red_y)), (int(self.green_x),int(self.green_y)), (0, 0, 0), 2)
#MidPoint_data = MidPoint()
#MidPoint_data.z = Middle_Point_Length
#self.MidPoint_pub.publish(MidPoint_data)
cv2.line(frame, (960, 1280), (960, 0), (0, 255, 0), 1)
cv2.line(frame, (0, 540), (1920, 540), (0, 255, 0), 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)