xiongyi/AutoControlSystem-git
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update 修改姿态

Browse Source
This commit is contained in:
FrankCV2048
2024-12-13 23:16:44 +08:00
committed by cdeyw
parent d231d9fcc8
commit 0751e6cafd
3 changed files with 59 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Trace/handeye_calibration.py
View File

@ -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] # 单位化方向向量