T2C = tlc.solve(poses_tool, poses_checker)
W2K = tlc.solve(poses_itool, poses_ichecker)
print 'Tool2Camera:', calc.matrix_to_rpypose(T2C)
print 'World2Checker:', calc.matrix_to_rpypose(W2K)
WHITE = (1, 1, 1)
RED = (1, 0, 0)
GREEN = (0, 1, 0)
BLUE = (0, 0, 1)
mplot3d = MPlot3D(scale=0.0025)
pp = laser_calibration.pattern_points
world_frame = calc.rpypose_to_matrix([0, 0, 0], [0, 0, 0])
mplot3d.draw_frame(calc.matrix_to_pose(world_frame), label='world_frame')
for k, tool_frame in enumerate(poses_tool):
WC = calc.matrix_compose((tool_frame, T2C))
mplot3d.draw_transformation(world_frame, tool_frame)
mplot3d.draw_transformation(tool_frame, WC, 'tool_pose%i' % k,
'camera_pose%i' % k)
WK = calc.matrix_compose((WC, poses_checker[k]))
# print 'Checker %i ->' % k, WK
# print np.allclose(W2K, WK, atol=0.0001)
mplot3d.draw_frame(calc.matrix_to_pose(WK))
mplot3d.draw_points(calc.points_transformation(WK, pp), color=GREEN)
mplot3d.draw_frame(calc.matrix_to_pose(W2K))
mplot3d.draw_points(calc.points_transformation(W2K, pp), color=WHITE)
img, grid = images[k], laser_calibration.grids[k]
if grid is not None:
chessboard_pose = pattern_poses[k]
profile3d, profile2d = laser_calibration.get_chessboard_laser(
img, grid, chessboard_pose)
import calculate as calc
arrow = np.float32([[0, 0, 0], [100, 0, 0]])
pose_arrow = calc.rpypose_to_pose((0, 0, 100), (0, np.deg2rad(90), 0))
trans_arrow = calc.matrix_invert(calc.pose_to_matrix(pose_arrow))
print 'arrow quat:', calc.matrix_to_quatpose(trans_arrow)
points = calc.transform_points(pose_arrow, arrow)
arrow2 = (points[0], points[1]-points[0])
print pose_arrow, points
position = ((0, 20, 0), (np.deg2rad(-45), np.deg2rad(45), np.deg2rad(0)))
pose = calc.rpypose_to_pose(*position)
print 'quat:', calc.rpypose_to_quatpose(*position)
trans = calc.matrix_compose([calc.pose_to_matrix(pose), calc.pose_to_matrix(pose_arrow)])
print 'final pose:', calc.matrix_to_quatpose(trans)
points = calc.transform_points(calc.matrix_to_pose(trans), arrow)
arrow3 = (points[0], points[1]-points[0])
print pose, points
mplot3d = MPlot3D()
mplot3d.draw_working_area(100, 100)
mplot3d.draw_frame((frame, point), label='frame')
mplot3d.draw_arrow(arrow)
#mplot3d.draw_arrow(arrow2)
mplot3d.draw_frame(calc.matrix_to_pose(trans_arrow))
mplot3d.draw_arrow(arrow3)
#mplot3d.draw_frame(calc.matrix_to_pose(calc.matrix_invert(trans)))
mplot3d.show()
T2C = tlc.solve(poses_tool, poses_checker)
W2K = tlc.solve(poses_itool, poses_ichecker)
print 'Tool2Camera:', calc.matrix_to_rpypose(T2C)
print 'World2Checker:', calc.matrix_to_rpypose(W2K)
WHITE = (1, 1, 1)
RED = (1, 0, 0)
GREEN = (0, 1, 0)
BLUE = (0, 0, 1)
mplot3d = MPlot3D(scale=0.0025)
pp = laser_calibration.pattern_points
world_frame = calc.rpypose_to_matrix([0, 0, 0], [0, 0, 0])
mplot3d.draw_frame(calc.matrix_to_pose(world_frame), label='world_frame')
for k, tool_frame in enumerate(poses_tool):
WC = calc.matrix_compose((tool_frame, T2C))
mplot3d.draw_transformation(world_frame, tool_frame)
mplot3d.draw_transformation(
tool_frame, WC, 'tool_pose%i' % k, 'camera_pose%i' % k)
WK = calc.matrix_compose((WC, poses_checker[k]))
# print 'Checker %i ->' % k, WK
# print np.allclose(W2K, WK, atol=0.0001)
mplot3d.draw_frame(calc.matrix_to_pose(WK))
mplot3d.draw_points(calc.points_transformation(WK, pp), color=GREEN)
mplot3d.draw_frame(calc.matrix_to_pose(W2K))
mplot3d.draw_points(calc.points_transformation(W2K, pp), color=WHITE)
img, grid = images[k], laser_calibration.grids[k]
if grid is not None:
chessboard_pose = pattern_poses[k]
profile3d, profile2d = laser_calibration.get_chessboard_laser(img, grid, chessboard_pose)
if len(profile2d) > 0:
poses_ichecker = [calc.matrix_invert(pose) for pose in poses_checker]
poses_itool = [calc.matrix_invert(pose) for pose in poses_tool]
print 'Hand Eye Calibration Solution'
tlc = HandEyeCalibration()
T2C = tlc.solve(poses_tool, poses_checker)
W2K = tlc.solve(poses_itool, poses_ichecker)
print 'Tool2Camera:', calc.matrix_to_rpypose(T2C)
print 'World2Checker:', calc.matrix_to_rpypose(W2K)
mplot3d = MPlot3D(scale=0.0025)
pp = laser_calibration.pattern_points
world_frame = calc.rpypose_to_matrix([0, 0, 0], [0, 0, 0])
#mplot3d.draw_frame(calc.matrix_to_pose(world_frame), label='world_frame')
for k, tool_frame in enumerate(poses_tool):
WC = calc.matrix_compose((tool_frame, T2C))
#mplot3d.draw_transformation(world_frame, tool_frame)
#mplot3d.draw_transformation(tool_frame, WC, 'tool_pose%i' % k,
# 'camera_pose%i' % k)
WK = calc.matrix_compose((WC, poses_checker[k]))
print 'Checker %i ->' % k, WK
print np.allclose(W2K, WK, atol=0.0001)
mplot3d.draw_frame(calc.matrix_to_pose(WK))
mplot3d.draw_points(calc.points_transformation(WK, pp),
color=(1, 1, 0))
mplot3d.draw_frame(calc.matrix_to_pose(W2K))
mplot3d.draw_points(calc.points_transformation(W2K, pp),
color=(1, 1, 1))
img, grid = images[k], laser_calibration.grids[k]
if grid is not None:
chessboard_pose = pattern_poses[k]