使用 opencv-python进行手眼标定

手眼标定整体流程及原理参考b站视频手眼标定-原理与实战,讲的十分详细。
这里完成的是eye-in-hand类型的手眼标定。

import cv2
import numpy as np
import glob
from math import *
import pandas as pd
import os

K=np.array([[4283.95148301687,-0.687179973528103,2070.79900757240],
            [0,4283.71915784510,1514.87274457919],
            [0,0,1]],dtype=np.float64)#大恒相机内参
chess_board_x_num=11#棋盘格x方向格子数
chess_board_y_num=8#棋盘格y方向格子数
chess_board_len=10#单位棋盘格长度,mm


#用于根据欧拉角计算旋转矩阵
def myRPY2R_robot(x, y, z):
    Rx = np.array([[1, 0, 0], [0, cos(x), -sin(x)], [0, sin(x), cos(x)]])
    Ry = np.array([[cos(y), 0, sin(y)], [0, 1, 0], [-sin(y), 0, cos(y)]])
    Rz = np.array([[cos(z), -sin(z), 0], [sin(z), cos(z), 0], [0, 0, 1]])
    R = Rz@Ry@Rx
    return R

#用于根据位姿计算变换矩阵
def pose_robot(x, y, z, Tx, Ty, Tz):
    thetaX = x / 180 * pi
    thetaY = y / 180 * pi
    thetaZ = z / 180 * pi
    R = myRPY2R_robot(thetaX, thetaY, thetaZ)
    t = np.array([[Tx], [Ty], [Tz]])
    RT1 = np.column_stack([R, t])  # 列合并
    RT1 = np.row_stack((RT1, np.array([0,0,0,1])))
    # RT1=np.linalg.inv(RT1)
    return RT1

#用来从棋盘格图片得到相机外参
def get_RT_from_chessboard(img_path,chess_board_x_num,chess_board_y_num,K,chess_board_len):
    '''
    :param img_path: 读取图片路径
    :param chess_board_x_num: 棋盘格x方向格子数
    :param chess_board_y_num: 棋盘格y方向格子数
    :param K: 相机内参
    :param chess_board_len: 单位棋盘格长度,mm
    :return: 相机外参
    '''
    img=cv2.imread(img_path)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    size = gray.shape[::-1]
    ret, corners = cv2.findChessboardCorners(gray, (chess_board_x_num, chess_board_y_num), None)
    # print(corners)
    corner_points=np.zeros((2,corners.shape[0]),dtype=np.float64)
    for i in range(corners.shape[0]):
        corner_points[:,i]=corners[i,0,:]
    # print(corner_points)
    object_points=np.zeros((3,chess_board_x_num*chess_board_y_num),dtype=np.float64)
    flag=0
    for i in range(chess_board_y_num):
        for j in range(chess_board_x_num):
            object_points[:2,flag]=np.array([(11-j-1)*chess_board_len,(8-i-1)*chess_board_len])
            flag+=1
    # print(object_points)

    retval,rvec,tvec  = cv2.solvePnP(object_points.T,corner_points.T, K, distCoeffs=None)
    # print(rvec.reshape((1,3)))
    # RT=np.column_stack((rvec,tvec))
    RT=np.column_stack(((cv2.Rodrigues(rvec))[0],tvec))
    RT = np.row_stack((RT, np.array([0, 0, 0, 1])))
    # RT=pose(rvec[0,0],rvec[1,0],rvec[2,0],tvec[0,0],tvec[1,0],tvec[2,0])
    # print(RT)

    # print(retval, rvec, tvec)
    # print(RT)
    # print('')
    return RT

folder = r"calib"#棋盘格图片存放文件夹
# files = os.listdir(folder)
# file_num=len(files)
# RT_all=np.zeros((4,4,file_num))

# print(get_RT_from_chessboard('calib/2.bmp', chess_board_x_num, chess_board_y_num, K, chess_board_len))
'''
这个地方很奇怪的特点,有些棋盘格点检测得出来,有些检测不了,可以通过函数get_RT_from_chessboard的运行时间来判断
'''
good_picture=[1,3,5,6,7,8,10,11,12,13,14,15]#存放可以检测出棋盘格角点的图片
# good_picture=[1,3,10,11,12]
file_num=len(good_picture)

#计算board to cam 变换矩阵
R_all_chess_to_cam_1=[]
T_all_chess_to_cam_1=[]
for i in good_picture:
    # print(i)
    image_path=folder+'/'+str(i)+'.bmp'
    RT=get_RT_from_chessboard(image_path, chess_board_x_num, chess_board_y_num, K, chess_board_len)

    # RT=np.linalg.inv(RT)

    R_all_chess_to_cam_1.append(RT[:3,:3])
    T_all_chess_to_cam_1.append(RT[:3, 3].reshape((3,1)))
# print(T_all_chess_to_cam.shape)

#计算end to base变换矩阵
file_address='calib/机器人基坐标位姿.xlsx'#从记录文件读取机器人六个位姿
sheet_1 = pd.read_excel(file_address)
R_all_end_to_base_1=[]
T_all_end_to_base_1=[]
# print(sheet_1.iloc[0]['ax'])
for i in good_picture:
    # print(sheet_1.iloc[i-1]['ax'],sheet_1.iloc[i-1]['ay'],sheet_1.iloc[i-1]['az'],sheet_1.iloc[i-1]['dx'],
    #                                   sheet_1.iloc[i-1]['dy'],sheet_1.iloc[i-1]['dz'])
    RT=pose_robot(sheet_1.iloc[i-1]['ax'],sheet_1.iloc[i-1]['ay'],sheet_1.iloc[i-1]['az'],sheet_1.iloc[i-1]['dx'],
                                      sheet_1.iloc[i-1]['dy'],sheet_1.iloc[i-1]['dz'])
    # RT=np.column_stack(((cv2.Rodrigues(np.array([[sheet_1.iloc[i-1]['ax']],[sheet_1.iloc[i-1]['ay']],[sheet_1.iloc[i-1]['az']]])))[0],
    #                    np.array([[sheet_1.iloc[i-1]['dx']],
    #                                   [sheet_1.iloc[i-1]['dy']],[sheet_1.iloc[i-1]['dz']]])))
    # RT = np.row_stack((RT, np.array([0, 0, 0, 1])))
    # RT = np.linalg.inv(RT)

    R_all_end_to_base_1.append(RT[:3, :3])
    T_all_end_to_base_1.append(RT[:3, 3].reshape((3, 1)))

# print(R_all_end_to_base_1)
R,T=cv2.calibrateHandEye(R_all_end_to_base_1,T_all_end_to_base_1,R_all_chess_to_cam_1,T_all_chess_to_cam_1)#手眼标定
RT=np.column_stack((R,T))
RT = np.row_stack((RT, np.array([0, 0, 0, 1])))#即为cam to end变换矩阵
print('相机相对于末端的变换矩阵为:')
print(RT)

#结果验证,原则上来说,每次结果相差较小
for i in range(len(good_picture)):

    RT_end_to_base=np.column_stack((R_all_end_to_base_1[i],T_all_end_to_base_1[i]))
    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((R_all_chess_to_cam_1[i],T_all_chess_to_cam_1[i]))
    RT_chess_to_cam=np.row_stack((RT_chess_to_cam,np.array([0,0,0,1])))
    # print(RT_chess_to_cam)

    RT_cam_to_end=np.column_stack((R,T))
    RT_cam_to_end=np.row_stack((RT_cam_to_end,np.array([0,0,0,1])))
    # print(RT_cam_to_end)

    RT_chess_to_base=RT_end_to_base@RT_cam_to_end@RT_chess_to_cam#即为固定的棋盘格相对于机器人基坐标系位姿
    RT_chess_to_base=np.linalg.inv(RT_chess_to_base)
    print('第',i,'次')
    print(RT_chess_to_base[:3,:])
    print('')

opencv-python参考文档里面关于这一块资料较少,函数的输入条件也试了好半天才试明白,归根结底还是要懂这个eye-in-hand的原理。

  • 8
    点赞
  • 61
    收藏
    觉得还不错? 一键收藏
  • 12
    评论
### 回答1: 用Python编写手眼标定流程需要按照以下步骤:1.设置标定板并确定其大小;2.使用摄像头对标定板进行拍摄,获取其标定点;3.使用Python编写程序,分析拍摄的图像,计算出标定点的位置;4.根据标定点的位置,计算出参考系的位置;5.记录参考系的位置,完成标定。 ### 回答2: 使用Python编写手眼标定流程可以通过以下步骤实现: 1. 导入所需的Python库:例如OpenCV、NumPy和Matplotlib。 2. 采集一组手眼标定数据:准备一个标定板,如棋盘格等,以及一个相机和一个机械臂或机器人。通过移动机械臂或机器人,采集多个不同位置和姿态下的标定板图像和相机位姿。 3. 检测标定板角点:使用OpenCV中的函数,如`cv2.findChessboardCorners()`,检测标定板图像中的所有角点位置。将检测到的角点保存在一个列表中。 4. 计算相机位姿:使用OpenCV中的函数,如`cv2.solvePnP()`,根据已知的标定板尺寸和检测到的标定板角点位置,计算相机的旋转矩阵和平移向量。 5. 计算机械臂位姿:根据机械臂或机器人的正运动学模型,根据已知的相机位姿和机械臂和相机的关系,计算机械臂的位姿(例如关节角度或末端执行器位置)。 6. 重复步骤2至5,直到采集足够数量的手眼标定数据。 7. 使用所有采集的相机位姿和机械臂位姿,进行手眼标定使用OpenCV中的函数,如`cv2.calibrateHandEye()`,根据所有相机位姿和机械臂位姿,计算相机和机械臂之间的转换矩阵。 8. 保存标定结果:将计算得到的相机和机械臂之间的转换矩阵保存到文件中,以备后续使用。 9. 可选:可视化结果:使用Matplotlib等库,将标定结果可视化,例如绘制相机和机械臂的位姿关系图。 以上是使用Python编写手眼标定流程的一般步骤。可以根据具体情况和需求进行相应的调整和扩展。 ### 回答3: 手眼标定是机器人领域中一个重要的问题,它通过识别机器人的手和眼之间的标定关系,为机器人的精确定位和控制提供基础。 使用Python编写手眼标定流程可以参考以下步骤: 1. 收集数据:准备好手和眼的坐标数据,并建立坐标系。可以记录机器人手部末端执行器的位姿,以及相机视野中标定板的位姿。 2. 定义问题:根据手和眼的坐标数据,建立手眼标定问题的数学模型。一般采用旋转矩阵和平移矩阵来表示手和眼之间的相对关系。 3. 优化算法:选择合适的优化算法,如最小二乘法,来计算手眼标定的最优解。使用Python的优化库,如Scipy或OpenCV,可以方便地实现这一步骤。 4. 数据处理:对收集到的数据进行预处理,如去除噪声、滤波等。可以使用Python中的NumPy和Pandas库来进行数据处理。 5. 可视化结果:使用Python中的Matplotlib库或者OpenCV库,将标定结果可视化展示出来,以便检查标定的准确性。 总结起来,使用Python编写手眼标定流程主要包括数据收集、问题定义、优化算法、数据处理和结果可视化等步骤。通过这些步骤,可以实现机器人的手眼标定,进而提高机器人的精确定位和控制能力。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 12
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值