After building a portion of your foundation, it’s best to continue learning by building something that’s not only useful, but also looks insanely impressive. So here you have it, a basic angle calculator.
UPDATE: WordPress is changing some of my code blocks to ‘amp’ and I haven’t yet found a way to fix this. For further guidance (although it would be a good exercise to infer), head over to my github repository.
LINK TO GITHUB GIST WITH ANGLECALC: https://gist.github.com/botforge/c88b842cafaa077a91048d51c2db0bdf
Prerequisites
This tutorial assumes you have some degree of proficiency with Python and can reasonably understand the OpenCV code here.
Determine HSV Range (again)
Before you continue writing the code you’ll need to use this HSV Trackbar to determine the Hue Low/High, Saturation Low/High and Value Low/High for the object you want to track. Mess around with the trackbars until you can only see the color of the object you are looking for. Repeat this process twice, for 2 differently colored objects. Note these values down, you will need them for later.
Filter for HSV Colors
Creating functions makes life a billion times easier, and allows you to organize your code much more effectively. I wrote the code initially with the functions findorange and findblue , although I eventually ended up using green and orange.
#import libs
import cv2
import numpy as np
import math
#uses distance formula to calculate distance
def distance((x1, y1), (x2,y2)):
dist = math.sqrt((math.fabs(x2-x1))**2+((math.fabs(y2-y1)))**2)
return dist
#filters for blue color and returns blue color position.
def findblue(frame):
maxcontour = None
blue = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
bluelow = np.array([55, 74, 0])#replace with your HSV Values
bluehi = np.array([74, 227, 255])#replace with your HSV Values
mask = cv2.inRange(blue, bluelow, bluehi)
res = cv2.bitwise_and(frame, frame, mask=mask)
#filters for orange color and returns orange color position.
def findorange(frame):
maxcontour = None
orange = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
orangelow = np.array([0, 142, 107])#replace with your HSV Values
orangehi = np.array([39, 255, 255])#replace with your HSV Values
mask = cv2.inRange(orange, orangelow, orangehi)
res = cv2.bitwise_and(frame, frame, mask=mask)
Just remember to change the bluelow and bluehi and orangelow and orangehi array’s elements to those that suit your color choice. All of the functions used should be familiar from my tutorial on ‘Object Tracking and Following with OpenCV Python‘; read that if you don’t get some of it. What we’ve essentially done is sent the initial frame as a parameter to each of these functions, where they then convert to HSV and threshold for the color.
Return object positions
Next, you want to continue building on findblue() and findorange() by allowing them to return the coordinates of your objects.
def findblue(frame):
blue = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
bluelow = np.array([55, 74, 0])#replace with your HSV Values
bluehi = np.array([74, 227, 255])#replace with your HSV Values
mask = cv2.inRange(blue, bluelow, bluehi)
res = cv2.bitwise_and(frame, frame, mask=mask)
cnts, hir = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
if len(cnts) >0:
maxcontour = max(cnts, key = cv2.contourArea)
#All this stuff about moments and M['m10'] etc.. are just to return center coordinates
M = cv2.moments(maxcontour)
if M['m00'] > 0 and cv2.contourArea(maxcontour)>2000:
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
return (cx, cy), True
else:
#(700,700), arbitrary random values that will conveniently not be displayed on screen
return (700,700), False
else:
return (700,700), False
#filters for orange color and returns orange color position.
def findorange(frame):
orange = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
orangelow = np.array([0, 142, 107])#replace with your HSV Values
orangehi = np.array([39, 255, 255])#replace with your HSV Values
mask = cv2.inRange(orange, orangelow, orangehi)
res = cv2.bitwise_and(frame, frame, mask=mask)
cnts, hir = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
if len(cnts) >0:
maxcontour = max(cnts, key = cv2.contourArea)
M = cv2.moments(maxcontour)
if M['m00'] > 0 and cv2.contourArea(maxcontour)>2000:
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
return (cx, cy), True
else:
return (700,700), False
else:
return (700,700), False
The cv2.Moments function and the cx and cy variable declarations are best explained in OpenCV’s introduction to contours. But simply put, lines n – n just return the coordinates of the center of the contour.
The reason we have the blogic boolean variable is just to validate that the object is present on screen. If the object isn’t present, this variable will be set to False, and the coordinates will be (700,700). I chose this set of points arbitrarily, as these, even when plotted, couldn’t be seen on my 300 x 400 window.
Distance Function
We’ll be using trigonometry to calculate the angle, so for this you’ll need to create a function that measures the distance between both points. For this, we use the standard distance equation you should’ve learnt in high school.
#uses distance formula to calculate distance
def distance((x1, y1), (x2,y2)):
dist = math.sqrt((math.fabs(x2-x1))**2+((math.fabs(y2-y1)))**2)
return dist
Main Loop
#capture video
cap = cv2.VideoCapture(0)
while(1):
_, frame = cap.read()
#if you're sending the whole frame as a parameter,easier to debug if you send a copy
fra = frame.copy()
#get coordinates of each object
(bluex, bluey), blogic = findblue(fra)
(orangex, orangey), ologic = findorange(fra)
#draw two circles around the objects (you can change the numbers as you like)
cv2.circle(frame, (bluex, bluey), 20, (255, 0, 0), -1)
cv2.circle(frame, (orangex, orangey), 20, (0, 128, 255), -1)
Our foundation is set. We’ve made a program that tracks the position of 2 different colored objects on screen, next, we need to apply trig to calculate the angle and display the entire setup in the most grandiose manner possible.
if blogic and ologic:
#quantifies the hypotenuse of the triangle
hypotenuse = distance((bluex,bluey), (orangex, orangey))
#quantifies the horizontal of the triangle
horizontal = distance((bluex, bluey), (orangex, bluey))
#makes the third-line of the triangle
thirdline = distance((orangex, orangey), (orangex, bluey))
#calculates the angle using trigonometry
angle = np.arcsin((thirdline/hypotenuse))* 180/math.pi
#draws all 3 lines
cv2.line(frame, (bluex, bluey), (orangex, orangey), (0, 0, 255), 2)
cv2.line(frame, (bluex, bluey), (orangex, bluey), (0, 0, 255), 2)
cv2.line(frame, (orangex,orangey), (orangex, bluey), (0,0,255), 2)
Our code is officially complete…sort of. If you run it, it’ll look great and work great but you’ll notice that it won’t detect any angle over 90 degrees. If you’re familiar with trig, you’ll know why, but to evade this situation and allow it to calculate angles until 180 degrees, we need a few more lines of code.
#Allows for calculation until 180 degrees instead of 90
if orangey < bluey and orangex > bluex:
cv2.putText(frame, str(int(angle)), (bluex-30, bluey), cv2.FONT_HERSHEY_SCRIPT_COMPLEX, 1, (0,128,220), 2)
elif orangey < bluey and orangex < bluex:
cv2.putText(frame, str(int(180 - angle)),(bluex-30, bluey), cv2.FONT_HERSHEY_SCRIPT_COMPLEX, 1, (0,128,220), 2)
elif orangey > bluey and orangex < bluex:
cv2.putText(frame, str(int(180 + angle)),(bluex-30, bluey), cv2.FONT_HERSHEY_SCRIPT_COMPLEX, 1, (0,128,220), 2)
elif orangey > bluey and orangex > bluex:
cv2.putText(frame, str(int(360 - angle)),(bluex-30, bluey), cv2.FONT_HERSHEY_SCRIPT_COMPLEX, 1, (0,128, 229), 2)
if k == ord('q'): break
And that’s it! If the object tracker didn’t impress already, now you have a live angle calculator using just your camera.
BOTFORGE 
