def read_pose(filename):
with open(filename, 'r') as f:
pose = eval(f.read())
tool_pose = calc.quatpose_to_matrix(*(np.array(pose[0]),
np.array(pose[1])))
return tool_pose
def read_pose(filename):
with open(filename, 'r') as f:
pose = eval(f.read())
tool_pose = calc.quatpose_to_matrix(*(np.array(pose[0]),
np.array(pose[1])))
return tool_pose
# mplot3d.draw_points(calc.points_transformation(WC, points3d[0:-1:50]), color=(1, 0, 0))
points3d = fit.apply_transformation(laser_profile.trans,
profiles[k])
# print points3d, laser_profile.trans
mplot3d.draw_points(calc.points_transformation(
WC, points3d[0:-1:25]),
color=GREEN)
mplot3d.show()
imgname = '../../data/fchecker.png'
posname = '../../data/pchecker.txt'
# Workobject
x, y, z = (1.596609, 0.101342, 0.936394
) # ERROR in this measure (z aprox 1.0)
qx, qy, qz, qw = (0.00185993, -0.00245949, -0.01122562, 0.99994981)
W2K = calc.quatpose_to_matrix(*(np.array([x, y, z]),
np.array([qx, qy, qz, qw])))
# rot1 = calc.rpypose_to_matrix(np.array([0, 0, 0]), np.array([0, 0, -np.pi/2]))
# rot2 = calc.rpypose_to_matrix(np.array([0, 0, 0]), np.array([0, np.pi, 0]))
# W2K = calc.matrix_compose((W2K, rot1, rot2))
W2T = read_pose(posname)
img = read_image(imgname)
grid = laser_calibration.find_chessboard(img)
pose = laser_calibration.get_chessboard_pose(grid)
C2K = calc.pose_to_matrix(pose)
K2C = calc.matrix_invert(C2K)
W2C = calc.matrix_compose((W2K, K2C))
T2W = calc.matrix_invert(W2T)
T2C = calc.matrix_compose((T2W, W2C))
print W2K
print W2T
print K2C
# points3d = laser_profile.profile_to_points3d(profiles[k],
# laser_profile.homography,
# laser_profile.pose)
# print points3d, laser_profile.pose, laser_profile.homography
# mplot3d.draw_points(calc.points_transformation(WC, points3d[0:-1:50]), color=(1, 0, 0))
points3d = fit.apply_transformation(laser_profile.trans, profiles[k])
# print points3d, laser_profile.trans
mplot3d.draw_points(calc.points_transformation(WC, points3d[0:-1:25]), color=GREEN)
mplot3d.show()
imgname = '../../data/frame_checker.png'
posname = '../../data/pose_checker.txt'
x, y, z = (1.6544, 0.0536, 0.9355) # ERROR in this measure (z aprox 1.0)
qx, qy, qz, qw = (0.002736, -0.004958, -0.004507, 0.999974)
W2K = calc.quatpose_to_matrix(*(np.array([x, y, z]), np.array([qx, qy, qz, qw])))
# rot1 = calc.rpypose_to_matrix(np.array([0, 0, 0]), np.array([0, 0, -np.pi/2]))
# rot2 = calc.rpypose_to_matrix(np.array([0, 0, 0]), np.array([0, np.pi, 0]))
# W2K = calc.matrix_compose((W2K, rot1, rot2))
W2T = read_pose(posname)
img = read_image(imgname)
grid = laser_calibration.find_chessboard(img)
pose = laser_calibration.get_chessboard_pose(grid)
C2K = calc.pose_to_matrix(pose)
K2C = calc.matrix_invert(C2K)
W2C = calc.matrix_compose((W2K, K2C))
T2W = calc.matrix_invert(W2T)
T2C = calc.matrix_compose((T2W, W2C))
print W2K
print W2T
print K2C
