diff --git a/Trace/handeye_calibration.py b/Trace/handeye_calibration.py index b8ba806..488bfc8 100644 --- a/Trace/handeye_calibration.py +++ b/Trace/handeye_calibration.py @@ -76,7 +76,7 @@ def getPosition(x,y,z,a,b,c,rotation,points): # 单位化方向向量 short_edge_direction = edge_vector / np.linalg.norm(edge_vector) - delta = -200#沿法向量方向抬高和压低,-指表示抬高,+值表示压低 + delta = 0#沿法向量方向抬高和压低,-指表示抬高,+值表示压低 angle = np.asarray([a,b,c]) noraml = camera2robot[:3, :3]@angle normal_vector = noraml / np.linalg.norm(noraml) @@ -86,4 +86,14 @@ def getPosition(x,y,z,a,b,c,rotation,points): return target_position,noraml_base +def getxyz(x,y,z,a,b,c): + target = np.asarray([x, y, z, 1]) + camera2robot = np.loadtxt('./Trace/com_pose2.txt', delimiter=' ') + target_position_raw = np.dot(camera2robot, target) + delta = -200 # 沿法向量方向抬高和压低,-指表示抬高,+值表示压低 + angle = np.asarray([a, b, c]) + noraml = camera2robot[:3, :3] @ angle + normal_vector = noraml / np.linalg.norm(noraml) + target_position = target_position_raw[:3] + delta * normal_vector + return target_position