RealsenseD415+MyCobot机械臂+aruco码位姿估计:python手眼标定

手眼标定代码

先上代码,希望大佬们帮我看一下是否有问题。
代码来源是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.        ]]

文章出处登录后可见!

已经登录?立即刷新

共计人评分,平均

到目前为止还没有投票!成为第一位评论此文章。

(0)
扎眼的阳光的头像扎眼的阳光普通用户
上一篇 2022年5月19日
下一篇 2022年5月19日

相关推荐