手眼标定代码
先上代码,希望大佬们帮我看一下是否有问题。
代码来源是CSDN上的代码,加上自己采集的数据。
python代码如下:
import cv2
import numpy as np
import transforms3d as tfs
import math
from scipy.spatial.transform import Rotation as R
# 标定板在相机坐标系下的平移 旋转向量
chess_to_cam = [
[-0.06621529 , 0.13094645 ,0.31150495, 2.7873504 , -0.70724874 , 0.01505296],
[-0.01520524 ,0.13752406 , 0.31428718, 2.52086289 ,-0.7563706 , 0.74143421],
[0.03576088, 0.13541224, 0.31811487, 2.46142008, -0.9906809, 0.72749194],
[0.08244047, 0.12495748, 0.31953674 ,2.46142008, -0.9906809, 0.72749194],
[0.12839916, 0.10395698, 0.3203565 ,2.24108505 ,-1.36912198, 0.66626523],
[0.1634839 ,0.08194832, 0.32214302, 1.67460169, -1.72986105, -1.17967643],
[-0.06986046 , 0.16068664, 0.38550589 ,3.04754189 ,-0.7445458 , 0.69108777],
[-0.01559573 , 0.16710656, 0.38562663, -2.84814259, 0.95005728 ,-0.74180944],
[0.03102854, 0.16386926 ,0.3882151 ,2.03512331 ,-1.1705569 , -0.39547647],
[0.08168991, 0.15306256, 0.39034941, -3.07134592 , 2.17851721 ,1.05950922],
[0.12979201, 0.13211736, 0.39186451, 1.66293978, -1.45852474 ,-0.86657837],
[0.17147111, 0.10379515, 0.39135897, -2.39255412, 2.5435715 , 1.78673278],
[-0.07081916, 0.13115815, 0.44020949 ,-2.84973128 , 0.72791037 ,-0.70020562],
[-0.02892028, 0.13882508, 0.44591697 ,-3.43403101, 1.48297145 , 0.38591057],
[0.01514055, 0.13714975 ,0.443343, -3.24444417 ,1.84363574 ,0.69163944],
[0.05637034, 0.12820875, 0.44279705 ,-3.0149895 , 2.07413218 , 1.00159165],
[0.09759316, 0.11109506, 0.44030735, -2.73936659 , 2.29504846 , 1.3497075 ],
[0.13321165, 0.08881823, 0.43955134 ,1.58211106, -1.60323079, -1.04295102],
]
# 机械臂末端在基坐标系下的姿态(x y z rx ry rz)
end_to_base = [
[0.1764, -0.0476, 0.3172, -24.53, 4.85, 47.02],
[0.1820, -0.0156, 0.3172, -24.53, 4.85, 57.21],
[0.1819, 0.0170, 0.3172, -24.47, 4.78, 67.52],
[0.1763, 0.0481, 0.3173, -24.67, 4.48, 77.46],
[0.1654, 0.0778, 0.3173, -24.67, 4.48, 87.39],
[0.1495, 0.1051, 0.3173, -24.67, 4.48, 97.32],
[0.1937, -0.0486, 0.2574, -8.64, 18.47, 50.02],
[0.1994, -0.0123, 0.2574, -8.64, 18.47, 60.57],
[0.1987, 0.0204, 0.2574, -8.64, 18.47, 69.98],
[0.1920, 0.0549, 0.2574, -8.64, 18.47, 80.08],
[0.1793, 0.0881, 0.2574, -8.64, 18.47, 90.28],
[0.1612, 0.1179, 0.2574, -8.64, 18.47, 100.3],
[0.1753, -0.0475, 0.2050, -4.13, 21.9, 51.93],
[0.1808, -0.0179, 0.2050, -4.13, 21.9, 61.43],
[0.1811, 0.0138, 0.2050, -4.13, 21.9, 71.45],
[0.1758, 0.0456, 0.2050, -4.13, 21.9, 81.64],
[0.1645, 0.0770, 0.2050, -4.13, 21.9, 92.19],
[0.1489, 0.1040, 0.2050, -4.13, 21.9, 102.03],
]
chess_to_cam_R,chess_to_cam_T = [],[]
end_to_base_R,end_to_base_T = [],[]
for chess_cam in chess_to_cam:
cc=chess_cam[3:6]
#print(cc)
cc_R, j = cv2.Rodrigues((cc[0],cc[1],cc[2]))
chess_to_cam_R.append(cc_R)
chess_to_cam_T.append(np.array(chess_cam[0:3]).reshape(3,1))
for end_base in end_to_base:
ed=end_base[3:6]
end_to_base_R.append(tfs.euler.euler2mat(math.radians(ed[0]),math.radians(ed[1]),math.radians(ed[2]),axes='sxyz')) #注意欧拉角顺序
end_to_base_T.append(np.array(end_base[0:3]).reshape(3,1))
print("---------------------------标定板RT---------------------")
#print(chess_to_cam_R)
#print(chess_to_cam_T)
print("---------------------------机械臂RT---------------------")
#print(end_to_base_R)
#print(end_to_base_T)
cam_to_end_R,cam_to_end_T = cv2.calibrateHandEye(end_to_base_R,end_to_base_T,chess_to_cam_R,chess_to_cam_T,
method=cv2.CALIB_HAND_EYE_TSAI)
print("---------------------------标定结果RT---------------------")
print(cam_to_end_R)
print(cam_to_end_T)
cam_to_end_RT = tfs.affines.compose(np.squeeze(cam_to_end_T), cam_to_end_R, [1, 1, 1])
print("标定结果:\n",cam_to_end_RT)
# 结果验证,原则上来说,每次结果相差较小
for i in range(0,16):
RT_end_to_base=np.column_stack((end_to_base_R[i],end_to_base_T[i].reshape(3,1)))
RT_end_to_base=np.row_stack((RT_end_to_base,np.array([0,0,0,1])))
# print(RT_end_to_base)
RT_chess_to_cam=np.column_stack((chess_to_cam_R[i],chess_to_cam_T[i].reshape(3,1)))
RT_chess_to_cam=np.row_stack((RT_chess_to_cam,np.array([0,0,0,1])))
# print(RT_chess_to_cam)
RT_chess_to_base=RT_end_to_base@cam_to_end_RT@RT_chess_to_cam #固定的棋盘格相对于机器人基坐标系位姿
# RT_chess_to_base=np.linalg.inv(RT_chess_to_base)
#print('第',i,'次')
#print(RT_chess_to_base)
#print('')
过程没有全部打印出来,结果:
---------------------------标定板RT---------------------
---------------------------机械臂RT---------------------
---------------------------标定结果RT---------------------
[[ 0.87185448 -0.21754658 -0.43879751]
[ 0.40325919 0.82730742 0.39108114]
[ 0.27794208 -0.51791497 0.80901934]]
[[-0.15808583]
[-0.27500843]
[ 0.49728589]]
标定结果:
[[ 0.87185448 -0.21754658 -0.43879751 -0.15808583]
[ 0.40325919 0.82730742 0.39108114 -0.27500843]
[ 0.27794208 -0.51791497 0.80901934 0.49728589]
[ 0. 0. 0. 1. ]]
宿舍环境比较乱,大概的场景搭建如下:
问题
希望有大佬,可以帮忙看看流程或者代码的问题。
我用最后的手眼矩阵,进行坐标转换的时候,总感觉不对。
而且用TASI方法和DANIILIDIS方法,得到的结果差异很大。
下面是DAN方法结果:
---------------------------标定板RT---------------------
---------------------------机械臂RT---------------------
---------------------------标定结果RT---------------------
[[-0.99554328 -0.07199065 -0.0609174 ]
[ 0.08498003 -0.40474112 -0.91047406]
[ 0.04088984 -0.9115931 0.40905507]]
[[ 0.05224488]
[ 0.58033807]
[-0.11443426]]
标定结果:
[[-0.99554328 -0.07199065 -0.0609174 0.05224488]
[ 0.08498003 -0.40474112 -0.91047406 0.58033807]
[ 0.04088984 -0.9115931 0.40905507 -0.11443426]
[ 0. 0. 0. 1. ]]
文章出处登录后可见!
已经登录?立即刷新