class Homography():
def __init__(self, pnts_pattern=np.float32([[0, 0], [0, 0], [0, 0], [0, 0]])):
self.process = Utils()
self.pnts_pattern = pnts_pattern
def read_image(self, image, scale=1):
size_y, size_x = image.shape[:2]
image_resized = cv2.resize(image, (size_x*scale, size_y*scale), interpolation=cv2.INTER_NEAREST)
# Show image and wait for 2 clicks.
cv2.imshow("Image", image_resized)
pts_image_resized = self.process.points_average(image_resized)
pts_image = pts_image_resized/scale
return pts_image
def calculate(self, pnts):
hom, status_1 = cv2.findHomography(pnts, self.pnts_pattern)
return hom
def scale(self, hom):
s = np.mean([hom[0, 0], hom[1, 1]])
return s
def transform(self, hom, pnts):
pnts = np.float32([
np.dot(hom, np.array([pnt[0], pnt[1], 1])) for pnt in pnts])
pnts = np.float32([pnt / pnt[2] for pnt in pnts])
return pnts[:, :2]
class TCP_to_cam():
def __init__(self):
self.process = Utils()
def read_image(self, image, scale):
# Read source image.
(size_x, size_y, colors) = image.shape
image_resized = cv2.resize(image, (size_x*scale, size_y*scale), interpolation=cv2.INTER_NEAREST)
# Show image and wait for 2 clicks.
cv2.imshow("Image", image_resized)
pts_image_resized = self.process.points_average(image_resized, 2)
pts_image = pts_image_resized/scale
return pts_image
def calculate_perp_bisector(self, pnts1, pnts2, pnts3, pnts4, pnts5, pnts6, pnts7):
pb = []
for k in range(len(pnts1)):
pb.append(Perp_bisector(pnts1[k], pnts3[k]))
pb.append(Perp_bisector(pnts2[k], pnts4[k]))
pb.append(Perp_bisector(pnts3[k], pnts5[k]))
pb.append(Perp_bisector(pnts4[k], pnts6[k]))
pb.append(Perp_bisector(pnts5[k], pnts7[k]))
pb.append(Perp_bisector(pnts6[k], pnts7[k]))
return pb
def calculate_intersection(self, pb_1, pb_2, pb_3, pb_4, pb_5, pb_6):
i_12 = Intersection(pb_1, pb_2)
i_23 = Intersection(pb_2, pb_3)
i_34 = Intersection(pb_3, pb_4)
i_46 = Intersection(pb_4, pb_6)
int_x = [i_12.x, i_23.x, i_34.x, i_46.x]
int_y = [i_12.y, i_23.y, i_34.y, i_46.y]
print 'X:', int_x
print 'Y:', int_y
x = np.mean(int_x)
y = np.mean(int_y)
return x, y
class TCP():
def __init__(self):
self.process = Utils()
def read_image(self, image, scale):
# Read source image.
(size_y, size_x, colors) = image.shape
image_resized = cv2.resize(image, (size_x*scale, size_y*scale), interpolation=cv2.INTER_NEAREST)
# Show image and wait for 2 clicks.
cv2.imshow("Image", image_resized)
pts_image_resized = self.process.points_average(image_resized, 1)
pts_image = pts_image_resized/scale
return pts_image
def calculate_perp_bisector(self, pxl_pnts):
pb = []
results = itertools.combinations(range(len(pxl_pnts)), 2)
# convert the combination iterator into a numpy array
combs = np.array(list(results))
for k in combs:
pb.append(Perp_bisector(pxl_pnts[k[0]][0], pxl_pnts[k[1]][0]))
return pb
def calculate_intersection(self, pbs):
intc = []
results = itertools.combinations(range(len(pbs)), 2)
# convert the combination iterator into a numpy array
combs = np.array(list(results))
for k in combs:
intc.append(Intersection(pbs[k[0]], pbs[k[1]]))
int_x = []
int_y = []
for i in intc:
int_x.append(i.x)
int_y.append(i.y)
# print "Intersection points: "
# print 'X:', int_x
# print 'Y:', int_y
# print " "
x = np.mean(int_x)
y = np.mean(int_y)
return x, y
def calculate_origin(self, pxl_pnts):
pbs = self.calculate_perp_bisector(pxl_pnts)
xc, yc = self.calculate_intersection(pbs)
pxl_origin = np.float32([[xc, yc]])
return pxl_origin
def calculate_translation_x(self, pnts1, pnts3, dist):
ds = []
for i in range(0, len(pnts1)):
ds_i = Translation(pnts1[i], pnts3[i])
ds.append(ds_i)
d_x = []
d_y = []
angle = []
s = []
for i in range(0, len(ds)):
d_x.append(ds[i].dx)
d_y.append(ds[i].dy)
if ds[i].dx < 0 and ds[i].dy < 0:
angle.append(math.atan(ds[i].dx/ds[i].dy) + math.pi/2)
elif ds[i].dx < 0 and ds[i].dy > 0:
angle.append(math.atan(ds[i].dx/ds[i].dy) - math.pi/2)
else:
angle.append(math.atan((-1)*ds[i].dy/ds[i].dx))
s.append(ds[i].s)
a = np.mean(angle)
s = np.mean(s)
factor = dist/s
print "Scale factor:", factor, "Angle:", math.degrees(a)
return a, factor
def calculate_translation_y(self, pnts1, pnts2, dist):
ds = []
for i in range(0, len(pnts1)):
ds_i = Translation(pnts1[i], pnts2[i])
ds.append(ds_i)
d_x = []
d_y = []
angle = []
s = []
for i in range(0, len(ds)):
d_x.append(ds[i].dx)
d_y.append(ds[i].dy)
if ds[i].dx > 0 and ds[i].dy < 0:
angle.append(math.atan((-1)*ds[i].dy/ds[i].dx) + math.pi/2)
elif ds[i].dx < 0 and ds[i].dy < 0:
angle.append(math.atan((-1)*ds[i].dy/ds[i].dx) - math.pi/2)
else:
angle.append(math.atan(ds[i].dx/ds[i].dy))
s.append(ds[i].s)
a = np.mean(angle)
s = np.mean(s)
factor = dist/s
print "Scale factor:", factor, "Angle:", math.degrees(a)
return a, factor
def calculate_orientation(self, pnts_origin, pnts_x, pnts_y, d_x, d_y):
angle_y, factor_a = self.calculate_translation_y(pnts_origin, pnts_y, d_y)
angle_x, factor_b = self.calculate_translation_x(pnts_origin, pnts_x, d_x)
factor = np.mean([factor_a, factor_b])
print " "
a_y = math.degrees(angle_y)
a_x = math.degrees(angle_x)
if -5 < (a_y - a_x) < 5:
angle_y = np.mean([angle_y, angle_x])
angle_x = angle_y
return factor, angle_y, angle_x
def calculate_matrix(self, xc, yc, a):
tcp_h_cam = [[math.cos(a), (-1)*math.sin(a), xc], [math.sin(a), math.cos(a), yc], [0, 0, 1]]
return tcp_h_cam
def transform(self, hom, pnts):
pnts = np.float32([
np.dot(hom, np.array([pnt[0], pnt[1], 1])) for pnt in pnts])
pnts = np.float32([pnt / pnt[2] for pnt in pnts])
return pnts[:, :2]
