手眼标定(eye-in-hand)-实现

目录

一、使用标定板类型

二、标定步骤及实现

2.1 相机外参标定

2.2 手眼标定

---

一、使用标定板类型

使用的是非对称圆分布的标定板如下图

二、标定步骤及实现

opencv实现

2.1 相机外参标定

相机外参标定结果点如下图，红色点为标定结果还原的点，绿色点为原始点

相机外参标定实现

def calibrate(img_path):
    #相机内参
    cameraMatrix =np.array([[2753.8901912306533,0,949.2910576185922],
                           [0,2753.5946107288023,593.6244109240248],
                           [0,0,1]])
    distCoeffs =np.array([
            -0.06501463142556438,
            0.1712297534594541,
            5.265462242831182e-05,
            0.00032355313360590214,
            0
        ])
    #标定板行列数
    pattern_shape=(4,5)
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
    #标定板上圆心的物理坐标定义（根据标定板的规格：间距为35mm）
    example_3d = np.array([[0,0,0],[70,0,0], [140,0,0],[210,0,0],
        [35,35,0],[105,35,0],[175,35,0],[245,35,0],
        [0,70,0],[70,70,0],[140,70,0],[210,70,0],
        [35,105,0],[105,105,0],[175,105,0],[245,105,0],
        [0,140,0],[70,140,0],[140,140,0],[210,140,0],
    ],dtype="float32")
    example_3d = example_3d*0.001#单位是米

    points_3d = []
    points_2d = []
    rvecsPnP, tvecsPnP = [], []
    images = glob.glob(img_path+'/*.jpg')  # 拍摄的十几张标定板图片所在目录
    for fname in images:
        # ret, frame = camera.cap.read()
        print(fname)
        frame = cv2.imread(fname)
        # assert ret
        gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        ret, bin = cv2.threshold(gray_frame, 171, 255, cv2.THRESH_BINARY)
        k = np.ones((5, 5), np.uint8)
        bin_CLOSE = cv2.morphologyEx(bin, cv2.MORPH_CLOSE, k)
        # cv2.imshow("bin", bin_CLOSE)
        # cv2.waitKey(0)
        params = cv2.SimpleBlobDetector_Params()
        params.filterByArea = True
        params.maxArea = 24034
        params.minArea = 961

        params.filterByColor = True
        params.filterByConvexity = False
        params.blobColor = 255
        params.minThreshold = 171
        params.maxThreshold = 251
        params.minDistBetweenBlobs = 50

        params.filterByInertia = True
        params.minInertiaRatio = 0.01

        blobDetector = cv2.SimpleBlobDetector_create(params)

        ret, corners = cv2.findCirclesGrid(
            bin_CLOSE, pattern_shape, flags=cv2.CALIB_CB_ASYMMETRIC_GRID ,blobDetector =blobDetector
        )

        if ret:
            corners2 = cv2.cornerSubPix(bin_CLOSE, corners, (11, 11), (-1, -1), criteria)
            points_3d.append(example_3d.copy())
            points_2d.append(corners2)
            print("Found calibration %i " % (len(points_3d)))
            drawn_frame = cv2.drawChessboardCorners(frame, pattern_shape, corners2, corners is not None)
            cv2.imwrite('drawn_frame.jpg',drawn_frame)
            # cv2.imshow("calib", drawn_frame)
            ret, camera_matrix, distortion_coefficients, rvecs, tvecs = cv2.calibrateCamera(points_3d, points_2d,
                                                                                            gray_frame.shape[::-1],
                                                                                            None, None)  #cameraMatrix, distCoeffs
            # print(camera_matrix)
            # print(distortion_coefficients)
            Ret, rvec, tvec = cv2.solvePnP(example_3d, corners2, cameraMatrix, distCoeffs)#,flags=cv2.SOLVEPNP_ITERATIVE
            # Ret ,rvec, tvec , _  = cv2.solvePnPGeneric(example_3d, corners2, cameraMatrix, distCoeffs)
            #将3*1欧拉角转为3*3旋转矩阵
            rvec, _ = cv2.Rodrigues(rvec)
            rvecsPnP.append(rvec)
            tvecsPnP.append(tvec)
            #将物理坐标还原回像素坐标查看标定精度
            imgpts, jac = cv2.projectPoints(points_3d[-1], rvec, tvec, cameraMatrix, distCoeffs)
            img = draw(frame, corners2, imgpts)#画图显示像素点和标定还原点
            #计算误差
            dis_sum= 0
            for i in range(0,len(corners2)):
                corner2 = corners2[i]
                imgpt = imgpts[i]

                dis = math.sqrt(np.sum(np.square(corner2-imgpt)))
                dis_sum += dis
                # print('点的误差为：',dis)
            print('平均误差为：',dis_sum/len(corners2))
            # cv2.imshow('',img)
            # cv2.waitKey(0)
            # # plt.show()
        # cv2.waitKey(10)
        # break


    # ret, camera_matrix, distortion_coefficients, rvecs0, tvecs0 = cv2.calibrateCamera(points_3d, points_2d, gray_frame.shape[::-1], None,None)#cameraMatrix, distCoeffs

    print('camera_matrix:\n',camera_matrix)
    assert ret
    return rvecsPnP, tvecsPnP,example_3d

2.2 手眼标定

手眼标定也使用opencv中的方法实现，代码如下

Rcf,Tcf = cv2.calibrateHandEye(Rfbs,Tfbs,Rtcs,Ttcs,method=cv2.CALIB_HAND_EYE_ANDREFF)