[2.5, -2.5],
[-2.5, 2.5],
[2.5, 2.5]])
pnts_final = np.float32([[0, 0],
[500, 0],
[0, 500],
[500, 500]])
im_ueye = cv2.imread('../../data/calibration/vis/frame07.jpg')
r1 = Representation()
#pxls_pattern_uEye = np.float32([[425.3, 108.7], [579.3, 118.0], [412.7, 262.0], [572.7, 276.0]])
pxls_pattern_uEye = np.float32([[579.3, 118.0], [572.7, 276.0], [425.3, 108.7], [412.7, 262.0]])
homography_uEye = homography.calculate(pxls_pattern_uEye)
print "uEye Homography: "
print homography_uEye
inv_homography_uEye = linalg.inv(homography_uEye)
pxl_TCP_uEye = np.float32([[320, 256]])
pnts_TCP_uEye = r1.transform(homography_uEye, pxl_TCP_uEye)[0]
print "TCP origin: ", pnts_TCP_uEye
a = np.deg2rad(0)
Frame_uEye = np.float32([[np.cos(a), -np.sin(a), pnts_TCP_uEye[0]],
[np.sin(a), np.cos(a), pnts_TCP_uEye[1]],
[0, 0, 1]])
inv_Frame_uEye = linalg.inv(Frame_uEye)
class Cal_camera():
def __init__(self, camera, pnts_pattern):
self.rep = Representation()
self.h = Homography(pnts_pattern)
self.tcp = TCP()
self.a = Angle()
self.camera = camera
self.hom = np.zeros((3, 3))
self.inv_hom = np.zeros((3, 3))
self.Frame = np.zeros((3, 3))
self.inv_Frame = np.zeros((3, 3))
self.hom_final_camera = np.zeros((3, 3))
def get_pxls_homography(self, image, scale=1):
pts_image = self.h.read_image(image, scale)
return pts_image
def calculate_homography(self, pxls_pattern):
self.hom = self.h.calculate(pxls_pattern)
self.inv_hom = linalg.inv(self.hom)
def get_pxl_origin(self, folder, scale=1):
pxl_pnts = []
for f in sorted(folder):
im_uEye = cv2.imread(f)
pxl_pnts.append(self.tcp.read_image(im_uEye, scale))
return pxl_pnts
def get_pxl_orientation(self, folder, scale=1):
for f in sorted(folder):
name = basename(f)
if name == "move_x.jpg":
im_uEye = cv2.imread(f)
pxl_pnts_x = self.tcp.read_image(im_uEye, scale)
elif name == "move_y.jpg":
im_uEye = cv2.imread(f)
pxl_pnts_y = self.tcp.read_image(im_uEye, scale)
elif name == "move_o.jpg":
im_uEye = cv2.imread(f)
pxl_pnts_origin = self.tcp.read_image(im_uEye, scale)
return pxl_pnts_origin, pxl_pnts_x, pxl_pnts_y
def calculate_TCP_orientarion(self, pxl_pnts, pxl_pnts_origin, pxl_pnts_x, pxl_pnts_y, dx, dy):
pxl_TCP = self.tcp.calculate_origin(pxl_pnts)
print "TCP :", pxl_TCP
factor, angle_y, angle_x = self.tcp.calculate_orientation(pxl_pnts_origin, pxl_pnts_x, pxl_pnts_y, dx, dy)
return pxl_TCP, factor, angle_y, angle_x
def calculate_angle_TCP(self, origin, axis_x, pattern):
pnt_origin = self.rep.transform(self.hom, origin)
pnt_x = self.rep.transform(self.hom, axis_x)
pnt_pattern = self.rep.transform(self.hom, pattern)
l_1 = np.float32([pnt_origin[0], pnt_x[0]])
l_2 = pnt_pattern[0:2]
vd1 = self.a.director_vector(l_1[0], l_1[1])
vd2 = self.a.director_vector(l_2[0], l_2[1])
angle = self.a.calculate_angle(vd1, vd2)
return angle
def calculate_frame(self, pxl_TCP, angle):
pnts_TCP = self.rep.transform(self.hom, pxl_TCP)[0]
a = np.deg2rad(angle)
self.Frame = np.float32([[np.cos(a), -np.sin(a), pnts_TCP[0]],
[np.sin(a), np.cos(a), pnts_TCP[1]],
[0, 0, 1]])
self.Orientation = np.float32([[np.cos(a), -np.sin(a), 0],
[np.sin(a), np.cos(a), 0],
[0, 0, 1]])
self.inv_Orientation = linalg.inv(self.Orientation)
self.inv_Frame = linalg.inv(self.Frame)
def calculate_hom_final(self, img, pnts, corners, pnts_final):
# pxls_camera = self.rep.transform(self.inv_hom, pnts)
im_measures = self.rep.define_camera(img.copy(), self.hom)
#------------------ Data in (c)mm
image_axis = self.rep.draw_axis_camera(im_measures, pnts)
#------------------
pnts_Frame = pnts
pnts_camera = self.rep.transform(self.Frame, pnts_Frame)
image_axis_tcp = self.rep.draw_axis_camera(image_axis, pnts_camera)
#------------------
corners_Frame = corners
corners_camera = self.rep.transform(self.Frame, corners_Frame)
img = self.rep.draw_points(image_axis_tcp, corners_camera)
#------------------
pxls_corner = self.rep.transform(self.inv_hom, corners_camera)
self.hom_final_camera, status = cv2.findHomography(pxls_corner.copy(), pnts_final)
self.write_config_file()
return self.hom_final_camera
def write_config_file(self):
hom_vis = self.hom_final_camera
hom_TCP = np.dot(self.inv_hom, self.Frame)
inv_hom_TCP = np.dot(self.inv_Frame, self.hom)
data = dict(
hom_vis=hom_vis.tolist(),
hom=hom_TCP.tolist(),
inv_hom=inv_hom_TCP.tolist(),
)
filename = '../../config/' + self.camera + '_config.yaml'
with open(filename, 'w') as outfile:
yaml.dump(data, outfile)
print data
def visualize_data(self):
print "Homography: "
print self.hom
print "Frame: "
print self.Frame
print "Final homography: "
print self.hom_final_camera
def draw_pattern_axis(self, pnts, img):
pnts_axis_pattern = pnts
pxls_axis_pattern = self.rep.transform(self.inv_hom, pnts_axis_pattern)
pnt_axis_pattern_final = self.rep.transform(self.hom_final_camera, pxls_axis_pattern)
img_pattern_NIT = self.rep.draw_axis_camera(img, pnt_axis_pattern_final)
return img_pattern_NIT
def draw_TCP_axis(self, pnts, img):
pnts_axis = pnts
pnts_axis_TCP = self.rep.transform(self.Frame, pnts_axis)
pxls_axis_TCP = self.rep.transform(self.inv_hom, pnts_axis_TCP)
pnt_axis_TCP_final = self.rep.transform(self.hom_final_camera, pxls_axis_TCP)
img_final_axis = self.rep.draw_axis_camera(img, pnt_axis_TCP_final)
return img_final_axis
def draw_axis(self, pnts, img):
img_TCP = self.draw_pattern_axis(pnts, img)
img_final = self.draw_TCP_axis(pnts, img_TCP)
return img_final
