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