diff --git a/Trace/handeye_calibration.py b/Trace/handeye_calibration.py index fb69af5..a224329 100644 --- a/Trace/handeye_calibration.py +++ b/Trace/handeye_calibration.py @@ -62,8 +62,8 @@ def getPosition(x,y,z,a,b,c,rotation,points): target_position = np.dot(camera2robot, target) corner_points_camera = np.asarray(points) - corner_points_base = np.dot(camera2robot[:3, :3], corner_points_camera.T).T + camera2robot[:3, 3] - edges = np.array([corner_points_base[i] - corner_points_base[i - 1] for i in range(len(corner_points_base))]) + corner_points_base = np.dot(T_BC[:3, :3], corner_points_camera.T).T + T_BC[:3, 3] + edges = np.array([corner_points_base[1] - corner_points_base[0]]) # for i in range(len(corner_points_base))]) edge_lengths = np.linalg.norm(edges, axis=1) min_edge_idx = np.argmin(edge_lengths) short_edge_direction = edges[min_edge_idx] / edge_lengths[min_edge_idx] # 单位化方向向量