# The inaccuracy of the floating point makes you not being able to calculate the correct angle between two vectors
from math import acos, sqrt
def angle_1(ux, vx, uy, vy):
eq = (ux*vx+uy*vy)/(sqrt(ux**2+uy**2)*sqrt(vx**2+vy**2))
return eq
def angle_2(ux, vx, uy, vy):
eq = sqrt((ux*vx+uy*vy)**2/((ux**2+uy**2)*(vx**2+vy**2)))
return eq
ux = 195
vx = 208
uy = -225
vy = -240
a = angle_1(195, 208, -225, -240)
b = angle_2(195, 208, -225, -240)
print('a: {0:.20f}'.format(a))
print('acos(a): {}'.format(acos(a)))
print('{0:.20f}'.format(b))
print('acos(b): {}'.format(acos(b)))
a = angle_1(287, 308, -82, -88)
b = angle_2(287, 308, -82, -88)
print('a: {0:.20f}'.format(a))
print('acos(a): {}'.format(acos(a)))
print('{0:.20f}'.format(b))
print('acos(b): {}'.format(acos(b)))
import cv2, copy, time, sys
sys.path.append('../../')
import numpy as np
from math import sqrt, sin, cos, pi, atan2
def display_img(img):
cv2.imshow('image', img)
show_window = True
while show_window:
k = cv2.waitKey(0) & 0xFF
if k == 27:#ESC
cv2.destroyAllWindows()
show_window = False
def get_colors_from_colormap(colormap=cv2.COLORMAP_JET, num_colors=1280, rgb_output=False):
gray_values = np.linspace(0, 255, num_colors, dtype=np.uint8)
gray_image = gray_values.reshape(-1, 1)
color_image = cv2.applyColorMap(gray_image, colormap)
colors = color_image.reshape(-1, 3).tolist()
return colors
def create_line_from_points():
a, b, c = 0.0183, 1, -169.1284
## fix the ray starting point:
all_edge_pts = []
w, h = 1280, 720
if abs(b) > 1e-5:
y_l=int(round( (-c)/b )) ## left, x=0
if (0<=y_l) and (y_l < h):
all_edge_pts.append([0,y_l])
y_r=int(round( (-a*(w-1)-c)/b )) ## right, x=w-1
if (0<=y_r) and (y_r < h):
all_edge_pts.append([w-1, y_r])
if abs(a) > 1e-5:
x_t = int(round( (-c)/a )) ## top, y=0
if (0<=x_t) and (x_t< w):
all_edge_pts.append([x_t, 0])
x_b = int(round( (-c-b*(h-1))/a )) ## bottom, y=h-1
if (0<=x_b) and (x_b < w):
all_edge_pts.append([x_b,h-1])
edge_pts = list({(x,y) for x,y in all_edge_pts}) ## remove duplicated points
if len(edge_pts) < 2:
print("ERROR! Found less than 2 points!")
temp_img = np.zeros((h,w), dtype=np.uint8)
cv2.line(temp_img, edge_pts[0], edge_pts[1], 255, 1)
ys, xs = np.where(temp_img==255)
line = list(zip(xs,ys))
sorted_line = copy.deepcopy(line)
sorted_line.sort()
return line, sorted_line
if __name__ == '__main__':
line, sorted_line = create_line_from_points()
img = np.full((720, 1280, 3), 255, dtype=np.uint8)
colors =get_colors_from_colormap(num_colors=1280)
for i in range(len(line)):
j = line[i]
k = sorted_line[i]
cv2.circle(img, j, radius=1, color=colors[i], thickness=-1)
cv2.circle(img, (k[0],k[1]+100), radius=1, color=colors[i], thickness=-1)
display_img(img)