91 lines
2.3 KiB
Python
91 lines
2.3 KiB
Python
import numpy as np
|
||
import cv2
|
||
import pickle
|
||
|
||
|
||
|
||
def pixel2robot(p_x, p_y, Zc=380):
|
||
"""
|
||
将像素坐标转换为机器人坐标系坐标
|
||
|
||
参数：
|
||
p_x (int)：像素坐标系x轴坐标
|
||
p_y (int)：像素坐标系y轴坐标
|
||
|
||
返回值：
|
||
robot_coords (numpy.ndarray)：机器人基坐标系下的坐标
|
||
|
||
示例：
|
||
>>> pixel2robot(100, 200)
|
||
array([-51.464, -41.334, 774.52])
|
||
"""
|
||
|
||
# 摄像头内参矩阵
|
||
camera_matrix = np.array([[1461.09955384437, 0, 963.862411991572],
|
||
[0, 1461.89533915958, 545.053909593624],
|
||
[0, 0, 1]])
|
||
|
||
# R = np.array([[-0.67004953, -0.74115346, 0.04153518],
|
||
# [0.73884813, -0.67127662, -0.05908594],
|
||
# [0.07167334, -0.00890232, 0.99738843]])
|
||
# print(R)
|
||
|
||
# T = np.array([0.06106497, -0.06272417, 0.7103077])
|
||
|
||
npzfile = np.load('camera_params.npz')
|
||
R = npzfile['R']
|
||
T = npzfile['T']
|
||
T = np.reshape(T, (3,))
|
||
|
||
# 336, 174
|
||
u, v = p_x, p_y
|
||
|
||
Zc = Zc
|
||
|
||
Xc = (u - camera_matrix[0, 2]) * Zc / camera_matrix[0, 0]
|
||
Yc = (v - camera_matrix[1, 2]) * Zc / camera_matrix[1, 1]
|
||
|
||
# 相机坐标系到标定板坐标系的转换
|
||
camera_coords = np.array([Xc, Yc, Zc])
|
||
base_coords = np.dot(R, camera_coords) + T
|
||
# print(base_coords)
|
||
|
||
taR = np.array([[1, 0, 0],
|
||
[0, -1, 0],
|
||
[0, 0, 0]])
|
||
|
||
taT = np.array([-15.5, -187, -30])
|
||
# print(np.dot(taR, base_coords))
|
||
base_coords = np.dot(taR, base_coords) + taT
|
||
# base_coords = base_coords + taT
|
||
|
||
return base_coords
|
||
|
||
|
||
# import cv2
|
||
# import numpy as np
|
||
|
||
|
||
# camera_matrix = np.array([[1461.09955384437, 0, 963.862411991572],
|
||
# [0, 1461.89533915958, 545.053909593624],
|
||
# [0, 0, 1]])
|
||
# dist_coeffs = np.array([0.123539164198816, -0.224109122219162, 0, 0, 0])
|
||
|
||
# def covert(point=[[0.0, 0.0]], z=260):
|
||
# point = np.array(point, dtype=np.float)
|
||
# pts_uv = cv2.undistortPoints(point, camera_matrix, dist_coeffs) * z
|
||
# return pts_uv[0][0]
|
||
|
||
# print(covert([993,10],260))
|
||
# print(covert([554,559],260))
|
||
|
||
# 485，300 [-135, -135.94, -30]
|
||
|
||
if __name__ == '__main__':
|
||
# print(pixel2robot(468, 254))
|
||
print(pixel2robot(485, 300))
|
||
# print(pixel2robot(956, 448))
|
||
|
||
|
||
|