基于Pycharm的YOLOv8杂草视觉检测2——ZED2相机环境配置+相机标定

目录

0. 前言

1. ZED2 相机简介及环境部署

1.1 ZED2相机 

1.2 ZED2相机环境搭建

1.3 小试牛刀——ZED2相机调用

 2. ZED2相机标定

2.1 张氏标定法

2.1.1 获取棋盘格图像

2.1.2 相机标定（单目）

（1）distortion.py求解畸变矫正系数

（2）extrinsics.py求解外参矩阵

（3）homography.py 求解单应性矩阵

（4）intrinsics.py求解内参矩阵

（5）refine_all微调参数

---

0. 前言

兄弟姐妹们大家好，举例上次的杂草视觉检测文章发布已经近10个多月了，本来想一心一意把杂草视觉检测算法优化并进行部署，但无奈期间事情太多，花了不少时间学了强弱电和智能控制，帮团队搭建了两套综合试验台，这几天才有时间玩一下深度相机，不过这段时间也踩过几个坑，为避免以后再犯同类错误，我把相机的环境配置、相机标定及运行代码写入这篇文章中。

关于YOLOV8的环境搭建及杂草数据集构建可以参考上篇文章：基于Pycharm的YOLOv8杂草视觉检测1——运行环境配置+杂草识别示例_pycharm yolov8-优快云博客https://blog.youkuaiyun.com/LSS_LSS/article/details/135025853?sharetype=blogdetail&sharerId=135025853&sharerefer=PC&sharesource=LSS_LSS&spm=1011.2480.3001.8118

1. ZED2 相机简介及环境部署

1.1 ZED2相机 

我在杂草识别需要考虑杂草距离传感器深度，因此需要用到双目视觉相机。目前我了解的较为常用的双目相机有：因特尔Intel Realsense D425/D435/D455、ZED mini/2i/2、OAK-D-Lite、Azure Kinect、奥比中光Gemini2、海康AGV等。鉴于我手头上有一台ZED2，我就以此开展介绍，大家想了解其它双目相机的具体参数可以访问官网或旗舰店。   

ZED2双目相机的五大优势如下：

• 双目深度感知：基于双目立体视觉，提供高精度的深度感知，适用于室内外场景。
• 广角视野：具有120°视野，覆盖范围更大，减少盲区。
• 高帧率：支持高达100帧/秒的深度测量，实时性强，适合动态场景。
• IMU融合：内置IMU传感器，提供更精确的姿态跟踪，适合SLAM应用。
• AI支持*：兼容深度学习算法，可进行目标识别、分割等高阶视觉任务。

ZED官方网址：ZED 2 - AI 立体相机 |立体实验室 (stereolabs.com)

ZED文档：Stereolabs 文档：API 参考、教程和集成

1.2 ZED2相机环境搭建

拿到ZED2相机，可不是插上电脑就能直接使用的哦，想要在Pycharm上运行ZED相机，就需要配置运行环境（就像新生报到，该注册的流程一样不能少），可以参考以下两个教程：

教你玩转ZED2双目摄像头_哔哩哔哩_bilibilihttps://www.bilibili.com/video/BV1814y1q7tu/?spm_id_from=333.999.0.0&vd_source=5f4b20d0e5c3da5309d6a393f8eb94a1

【ZED】从零开始使用ZED相机（一）：windows下的安装配置与测试-优快云博客https://blog.youkuaiyun.com/qq_44703886/article/details/122695129?ops_request_misc=%257B%2522request%255Fid%2522%253A%25225888DA10-3961-47AB-975E-E558BB543876%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=5888DA10-3961-47AB-975E-E558BB543876&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~top_positive~default-1-122695129-null-null.142%5Ev100%5Epc_search_result_base2&utm_term=zed%E7%9B%B8%E6%9C%BA&spm=1018.2226.3001.4187

1.3 小试牛刀——ZED2相机调用

在训练好的杂草识别模型项目中，新建.py文件，键入以下内容：

from ultralytics import YOLO
import cv2
from cv2 import getTickCount, getTickFrequency

yolo = YOLO('C:/Users/LSs/PycharmProjects/yazhicao622_1/ultralytics-main/runs/detect/train5/weights/best.pt') # 训练好的.best模型绝对路径

# 摄像头实时检测

cap = cv2.VideoCapture(0)
while cap.isOpened():
    loop_start = getTickCount()  # 记录循环开始的时间，用于计算每一帧的处理时间
    success, frame = cap.read()  # 读取摄像头的一帧图像

    if success:
        results = yolo.predict(source=frame)  # 对当前帧进行目标检测并显示结果
    annotated_frame = results[0].plot()  # 将检测结果绘制在图像上，得到带有目标框的图像。

    # 显示程序
    loop_time = getTickCount() - loop_start  # 计算处理一帧图像所花费的时间。
    total_time = loop_time / (getTickFrequency())  # 将处理时间转换为秒数。
    FPS = int(1 / total_time)  # FPS计算
    # 在图像左上角添加FPS文本
    fps_text = f"FPS: {FPS:.2f}"  # 构造显示帧率的文本字符串
    font = cv2.FONT_HERSHEY_SIMPLEX  # 选择字体类型
    font_scale = 1  # 设置字体的缩放比例
    font_thickness = 2  # 设置字体的粗细
    text_color = (0, 0, 255)  # 红色
    text_position = (10, 30)  # 左上角位置

    cv2.putText(annotated_frame, fps_text, text_position, font, font_scale, text_color, font_thickness)
    cv2.imshow('Real-time detection', annotated_frame)
    # 通过按下 'q' 键退出循环
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

cap.release()  # 释放摄像头资源
cv2.destroyAllWindows()  # 关闭OpenCV窗口

检测效果为视频所示：

杂草视觉检测

 2. ZED2相机标定

2.1 张氏标定法

2.1.1 获取棋盘格图像

我手里从同学那边借到了一张12*9的棋盘格标定板，内角点数为11*8（记住这个数据，程序中还要用到），就以此为基础开展标定。手里没有棋盘格标定板的，可以打印一张12*9的棋盘格（A4或A3均可）。我给大家准备了四种款式：棋盘、查鲁科、口径/AprilGird和圆，大家通过链接自取。当然，也可以通过第二个链接生成自己所需的标定板样式：

阿里云盘分享https://www.alipan.com/s/6MPAFC7Hk4wCamera Calibration Pattern Generator – calib.iohttps://calib.io/pages/camera-calibration-pattern-generator

以下内容参考了：【ZED】从零开始使用ZED相机（三）：相机标定（张氏标定法）_zed相机教程-优快云博客https://blog.youkuaiyun.com/qq_44703886/article/details/122767064将ZED可能调用的函数封装成一个类，初始化ZED相机以及自定义捕获图像的函数，代码如下：

import pyzed.sl as sl
import cv2
import numpy as np
import os
class CameraZed2:
    def __init__(self,resolution=None,fps=30,depthMode = None):
        self.zed = sl.Camera()
        self.input_type = sl.InputType()
        self.init_params = sl.InitParameters(input_t=self.input_type)
        # 设置分辨率
        if resolution == "2K":
            self.init_params.camera_resolution = sl.RESOLUTION.HD2K
        elif resolution == "1080":
            self.init_params.camera_resolution = sl.RESOLUTION.HD1080
        else:  # 默认
            self.init_params.camera_resolution = sl.RESOLUTION.HD720
        self.init_params.camera_fps = fps  # 设置帧率
        # 设置获取深度信息的模式
        if depthMode == "PERFORMANCE":
            self.init_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE
        elif depthMode == "QUALITY":
            self.init_params.depth_mode = sl.DEPTH_MODE.QUALITY
        else:
            self.init_params.depth_mode = sl.DEPTH_MODE.ULTRA
        self.init_params.coordinate_units = sl.UNIT.MILLIMETER  # 单位毫米
        # 打开相机
        err = self.zed.open(self.init_params)
        if err != sl.ERROR_CODE.SUCCESS:
            print(repr(err))
            self.zed.close()
            exit(1)

        self.runtime = sl.RuntimeParameters()
        self.runtime.sensing_mode = sl.SENSING_MODE.STANDARD
        self.savepath = ''  # 标定图像保存的路径

    def grab_imgs(self):  # 捕获左右图像用于相机标定（文件夹自动创建）
        img_l = sl.Mat()
        img_r = sl.Mat()
        num = 0
        # 自动创建保存文件夹
        import time
        name = time.strftime("%Y-%m-%d-%H-%M", time.localtime())
        self.savepath = './images/%s'% name
        if not os.path.exists(savepath):
             os.mkdir(savepath)

        while True:
            if self.zed.grab(self.runtime) == sl.ERROR_CODE.SUCCESS:
                self.zed.retrieve_image(img_l,sl.VIEW.LEFT)
                self.img_l = img_l.get_data()
                self.zed.retrieve_image(img_r,sl.VIEW.RIGHT)
                self.img_r = img_r.get_data()
                view = np.concatenate((self.img_l,self.img_r),axis=1)
                cv2.imshow('View',cv2.resize(view,(1920,540)))
                key = cv2.waitKey(1)
                if key & 0xFF == ord('s'):
                    savePath = os.path.join(self.savepath, "L{:0>3d}.png".format(num))
                    cv2.imwrite(savePath, self.img_l)
                    # savePath = os.path.join(self.savepath, "R{:0>3d}.png".format(num))
                    # cv2.imwrite(savePath, self.img_r)
                    num +=1
                if key & 0xFF == 27:
                    break

调用类

if __name__ == "__main__":
    cam = CameraZed2(resolution='1080',fps=30)
    cam.grab_imgs()  # 获取标定图像

拍摄时，相机进行固定，标定板与相机的距离相对稳定且标定板面积应占总像素面积的60%以上，棋盘格尽量全部呈现。获取到的棋盘格照片如下：

2.1.2 相机标定（单目）

继续在上述的类中新增定义相机标定函数

新增的函数定义如下：

    def calibrate(self,pic_points,real_points_x_y,real_points):
        from step.homography import get_homography
        from step.intrinsics import get_intrinsics_param
        from step.extrinsics import get_extrinsics_param
        from step.distortion import get_distortion
        from step.refine_all import refinall_all_param
        # 求单应矩阵
        H = get_homography(pic_points, real_points_x_y)
        # 求内参
        intrinsics_param = get_intrinsics_param(H)
        # 求对应每幅图外参
        extrinsics_param = get_extrinsics_param(H, intrinsics_param)
        # 畸变矫正
        k = get_distortion(intrinsics_param, extrinsics_param, pic_points, real_points_x_y)
        # 微调所有参数
        [new_intrinsics_param, new_k, new_extrinsics_param] = refinall_all_param(intrinsics_param,
                                                                                 k, extrinsics_param, real_points,
                                                                                 pic_points)
        print("Homographic矩阵：\n",H)
        print("内参矩阵:\n", new_intrinsics_param)
        print("畸变校正系数:\n", new_k)
        print("外参矩阵:\n", new_extrinsics_param)
    # 相机标定执行函数
    def cal_exe(self):  # 执行校正
        file_dir = r'./images'
        # 标定所用图像
        pic_name = os.listdir(file_dir)

        # 由于棋盘为二维平面，设定世界坐标系在棋盘上，一个单位代表一个棋盘宽度，产生世界坐标系三维坐标
        cross_corners = [8, 11]  # 棋盘方块交界点排列,8行11列
        real_coor = np.zeros((cross_corners[0] * cross_corners[1], 3), np.float32)
        real_coor[:, :2] = np.mgrid[0:8, 0:11].T.reshape(-1, 2)

        real_points = []
        real_points_x_y = []
        pic_points = []

        for pic in pic_name:
            pic_path = os.path.join(file_dir, pic)
            pic_data = cv2.imread(pic_path)
            # 寻找到棋盘角点
            succ, pic_coor = cv2.findChessboardCorners(pic_data, (cross_corners[0], cross_corners[1]), None)
            # 显示角点
            cv2.drawChessboardCorners(pic_data, (cross_corners[0], cross_corners[1]), pic_coor, succ)
            cv2.namedWindow('View')
            cv2.imshow("View", pic_data)
            cv2.waitKey(0)
            cv2.destroyAllWindows()
            if succ:
                # 添加每幅图的对应3D-2D坐标
                pic_coor = pic_coor.reshape(-1, 2)
                pic_points.append(pic_coor)  # 图像角点坐标(48,2)
                real_points.append(real_coor)  # 真实角点的坐标(48,3),相对坐标，其中Z=0
                real_points_x_y.append(real_coor[:, :2])  # 真实角点的X,Y
        self.calibrate(pic_points,real_points_x_y,real_points)

注意，上述代码中导入了五个文件。

  from step.homography import get_homography
  from step.intrinsics import get_intrinsics_param
  from step.extrinsics import get_extrinsics_param
  from step.distortion import get_distortion
  from step.refine_all import refinall_all_param

在该代码的同根目录下新建文件夹step，在文件夹中放入如下文件：

1 distortion.py求解畸变矫正系数
2 extrinsics.py求解外参矩阵
3 homography.py 求解单应性矩阵
4 intrinsics.py求解内参矩阵
5 refine_all微调参数

（1）distortion.py求解畸变矫正系数

"""
20240916
distortion.py求解畸变矫正系数
"""
import numpy as np


# 返回畸变矫正系数k0,k1
def get_distortion(intrinsic_param, extrinsic_param, pic_coor, real_coor):
    D = []
    d = []
    for i in range(len(pic_coor)):
        for j in range(len(pic_coor[i])):
            # 转换为齐次坐标
            single_coor = np.array([(real_coor[i])[j, 0], (real_coor[i])[j, 1], 0, 1])

            # 利用现有内参及外参求出估计图像坐标
            u = np.dot(np.dot(intrinsic_param, extrinsic_param[i]), single_coor)
            [u_estim, v_estim] = [u[0] / u[2], u[1] / u[2]]

            coor_norm = np.dot(extrinsic_param[i], single_coor)
            coor_norm /= coor_norm[-1]

            # r = np.linalg.norm((real_coor[i])[j])
            r = np.linalg.norm(coor_norm)

            D.append(np.array([(u_estim - intrinsic_param[0, 2]) * r ** 2, (u_estim - intrinsic_param[0, 2]) * r ** 4]))
            D.append(np.array([(v_estim - intrinsic_param[1, 2]) * r ** 2, (v_estim - intrinsic_param[1, 2]) * r ** 4]))

            # 求出估计坐标与真实坐标的残差
            d.append(pic_coor[i][j, 0] - u_estim)
            d.append(pic_coor[i][j, 1] - v_estim)
            '''

            D.append(np.array([(pic_coor[i][j, 0] - intrinsic_param[0, 2]) * r ** 2, (pic_coor[i][j, 0] - intrinsic_param[0, 2]) * r ** 4]))
            D.append(np.array([(pic_coor[i][j, 1] - intrinsic_param[1, 2]) * r ** 2, (pic_coor[i][j, 1] - intrinsic_param[1, 2]) * r ** 4]))

            #求出估计坐标与真实坐标的残差
            d.append(u_estim - pic_coor[i][j, 0])
            d.append(v_estim - pic_coor[i][j, 1])
            '''

    D = np.array(D)
    temp = np.dot(np.linalg.inv(np.dot(D.T, D)), D.T)
    k = np.dot(temp, d)
    '''
    #也可利用SVD求解D * k = d中的k
    U, S, Vh=np.linalg.svd(D, full_matrices=False)
    temp_S = np.array([[S[0], 0],
                       [0, S[1]]])
    temp_res = np.dot(Vh.transpose(), np.linalg.inv(temp_S))
    temp_res_res = np.dot(temp_res, U.transpose())
    k = np.dot(temp_res_res, d)
    '''
    return k

（2）extrinsics.py求解外参矩阵

"""
20240916
extrinsics.py求解外参矩阵
"""

import numpy as np

#返回每一幅图的外参矩阵[R|t]
def get_extrinsics_param(H, intrinsics_param):
    extrinsics_param = []

    inv_intrinsics_param = np.linalg.inv(intrinsics_param)
    for i in range(len(H)):
        h0 = (H[i].reshape(3, 3))[:, 0]
        h1 = (H[i].reshape(3, 3))[:, 1]
        h2 = (H[i].reshape(3, 3))[:, 2]

        scale_factor = 1 / np.linalg.norm(np.dot(inv_intrinsics_param, h0))

        r0 = scale_factor * np.dot(inv_intrinsics_param, h0)
        r1 = scale_factor * np.dot(inv_intrinsics_param, h1)
        t = scale_factor * np.dot(inv_intrinsics_param, h2)
        r2 = np.cross(r0, r1)

        R = np.array([r0, r1, r2, t]).transpose()
        extrinsics_param.append(R)

    return extrinsics_param

（3）homography.py 求解单应性矩阵

"""
20240916
homography.py 求解单应性矩阵
"""

import numpy as np
from scipy import optimize as opt


#求输入数据的归一化矩阵
def normalizing_input_data(coor_data):
    x_avg = np.mean(coor_data[:, 0])
    y_avg = np.mean(coor_data[:, 1])
    sx = np.sqrt(2) / np.std(coor_data[:, 0])
    sy = np.sqrt(2) / np.std(coor_data[:, 1])

    norm_matrix = np.matrix([[sx, 0, -sx * x_avg],
                             [0, sy, -sy * y_avg],
                             [0, 0, 1]])
    return norm_matrix


#求取初始估计的单应矩阵
def get_initial_H(pic_coor, real_coor):
    # 获得归一化矩阵
    pic_norm_mat = normalizing_input_data(pic_coor)
    real_norm_mat = normalizing_input_data(real_coor)

    M = []
    for i in range(len(pic_coor)):
        #转换为齐次坐标
        single_pic_coor = np.array([pic_coor[i][0], pic_coor[i][1], 1])
        single_real_coor = np.array([real_coor[i][0], real_coor[i][1], 1])

        #坐标归一化
        pic_norm = np.dot(pic_norm_mat, single_pic_coor)
        real_norm = np.dot(real_norm_mat, single_real_coor)

        #构造M矩阵
        M.append(np.array([-real_norm.item(0), -real_norm.item(1), -1,
                      0, 0, 0,
                      pic_norm.item(0) * real_norm.item(0), pic_norm.item(0) * real_norm.item(1), pic_norm.item(0)]))

        M.append(np.array([0, 0, 0,
                      -real_norm.item(0), -real_norm.item(1), -1,
                      pic_norm.item(1) * real_norm.item(0), pic_norm.item(1) * real_norm.item(1), pic_norm.item(1)]))

    #利用SVD求解M * h = 0中h的解
    U, S, VT = np.linalg.svd((np.array(M, dtype='float')).reshape((-1, 9)))
    # 最小的奇异值对应的奇异向量,S求出来按大小排列的，最后的最小
    H = VT[-1].reshape((3, 3))
    H = np.dot(np.dot(np.linalg.inv(pic_norm_mat), H), real_norm_mat)
    H /= H[-1, -1]

    return H


#返回估计坐标与真实坐标偏差
def value(H, pic_coor, real_coor):
    Y = np.array([])
    for i in range(len(real_coor)):
        single_real_coor = np.array([real_coor[i, 0], real_coor[i, 1], 1])
        U = np.dot(H.reshape(3, 3), single_real_coor)
        U /= U[-1]
        Y = np.append(Y, U[:2])

    Y_NEW = (pic_coor.reshape(-1) - Y)

    return Y_NEW


#返回对应jacobian矩阵
def jacobian(H, pic_coor, real_coor):
    J = []
    for i in range(len(real_coor)):
        sx = H[0]*real_coor[i][0] + H[1]*real_coor[i][1] +H[2]
        sy = H[3]*real_coor[i][0] + H[4]*real_coor[i][1] +H[5]
        w = H[6]*real_coor[i][0] + H[7]*real_coor[i][1] +H[8]
        w2 = w * w

        J.append(np.array([real_coor[i][0]/w, real_coor[i][1]/w, 1/w,
                           0, 0, 0,
                           -sx*real_coor[i][0]/w2, -sx*real_coor[i][1]/w2, -sx/w2]))

        J.append(np.array([0, 0, 0,
                           real_coor[i][0]/w, real_coor[i][1]/w, 1/w,
                           -sy*real_coor[i][0]/w2, -sy*real_coor[i][1]/w2, -sy/w2]))

    return np.array(J)


#利用Levenberg Marquart算法微调H
def refine_H(pic_coor, real_coor, initial_H):
    initial_H = np.array(initial_H)
    final_H = opt.leastsq(value,
                          initial_H,
                          Dfun=jacobian,
                          args=(pic_coor, real_coor))[0]

    final_H /= np.array(final_H[-1])
    return final_H


#返回微调后的H
def get_homography(pic_coor, real_coor):
    refined_homographies =[]

    error = []
    for i in range(len(pic_coor)):
        initial_H = get_initial_H(pic_coor[i], real_coor[i])
        final_H = refine_H(pic_coor[i], real_coor[i], initial_H)
        refined_homographies.append(final_H)

    return np.array(refined_homographies)

（4）intrinsics.py求解内参矩阵

"""
20240916
intrinsics.py求解内参矩阵
"""

import numpy as np


#返回pq位置对应的v向量
def create_v(p, q, H):
    H = H.reshape(3, 3)
    return np.array([
        H[0, p] * H[0, q],
        H[0, p] * H[1, q] + H[1, p] * H[0, q],
        H[1, p] * H[1, q],
        H[2, p] * H[0, q] + H[0, p] * H[2, q],
        H[2, p] * H[1, q] + H[1, p] * H[2, q],
        H[2, p] * H[2, q]
    ])


#返回相机内参矩阵A
def get_intrinsics_param(H):
    #构建V矩阵
    V = np.array([])
    for i in range(len(H)):
        V = np.append(V, np.array([create_v(0, 1, H[i]), create_v(0, 0 , H[i])- create_v(1, 1 , H[i])]))

    #求解V*b = 0中的b
    U, S, VT = np.linalg.svd((np.array(V, dtype='float')).reshape((-1, 6)))
    #最小的奇异值对应的奇异向量,S求出来按大小排列的，最后的最小
    b = VT[-1]

    #求取相机内参
    w = b[0] * b[2] * b[5] - b[1] * b[1] * b[5] - b[0] * b[4] * b[4] + 2 * b[1] * b[3] * b[4] - b[2] * b[3] * b[3]
    d = b[0] * b[2] - b[1] * b[1]

    alpha = np.sqrt(w / (d * b[0]))
    beta = np.sqrt(w / d**2 * b[0])
    gamma = np.sqrt(w / (d**2 * b[0])) * b[1]
    uc = (b[1] * b[4] - b[2] * b[3]) / d
    vc = (b[1] * b[3] - b[0] * b[4]) / d

    return np.array([
        [alpha, gamma, uc],
        [0,     beta,  vc],
        [0,     0,      1]
    ])

（5）refine_all微调参数

"""
20240916
refine_all微调参数
"""

import numpy as np
import math
from scipy import optimize as opt

#微调所有参数
def refinall_all_param(A, k, W, real_coor, pic_coor):
    #整合参数
    P_init = compose_paramter_vector(A, k, W)

    #复制一份真实坐标
    X_double = np.zeros((2 * len(real_coor) * len(real_coor[0]), 3))
    Y = np.zeros((2 * len(real_coor) * len(real_coor[0])))

    M = len(real_coor)
    N = len(real_coor[0])
    for i in range(M):
        for j in range(N):
            X_double[(i * N + j) * 2] = (real_coor[i])[j]
            X_double[(i * N + j) * 2 + 1] = (real_coor[i])[j]
            Y[(i * N + j) * 2] = (pic_coor[i])[j, 0]
            Y[(i * N + j) * 2 + 1] = (pic_coor[i])[j, 1]

    #微调所有参数
    P = opt.leastsq(value,
                    P_init,
                    args=(W, real_coor, pic_coor),
                    Dfun=jacobian)[0]

    #raial_error表示利用标定后的参数计算得到的图像坐标与真实图像坐标点的平均像素距离
    error = value(P, W, real_coor, pic_coor)
    raial_error = [np.sqrt(error[2 * i]**2 + error[2 * i + 1]**2) for i in range(len(error) // 2)]

    print("total max error:\t", np.max(raial_error))

    #返回拆解后参数，分别为内参矩阵，畸变矫正系数，每幅图对应外参矩阵
    return decompose_paramter_vector(P)


#把所有参数整合到一个数组内
def compose_paramter_vector(A, k, W):
    alpha = np.array([A[0, 0], A[1, 1], A[0, 1], A[0, 2], A[1, 2], k[0], k[1]])
    P = alpha
    for i in range(len(W)):
        R, t = (W[i])[:, :3], (W[i])[:, 3]

        #旋转矩阵转换为一维向量形式
        zrou = to_rodrigues_vector(R)

        w = np.append(zrou, t)
        P = np.append(P, w)
    return P


#分解参数集合，得到对应的内参，外参，畸变矫正系数
def decompose_paramter_vector(P):
    [alpha, beta, gamma, uc, vc, k0, k1] = P[0:7]
    A = np.array([[alpha, gamma, uc],
                  [0, beta, vc],
                  [0, 0, 1]])
    k = np.array([k0, k1])
    W = []
    M = (len(P) - 7) // 6

    for i in range(M):
        m = 7 + 6 * i
        zrou = P[m:m+3]
        t = (P[m+3:m+6]).reshape(3, -1)

        #将旋转矩阵一维向量形式还原为矩阵形式
        R = to_rotation_matrix(zrou)

        #依次拼接每幅图的外参
        w = np.concatenate((R, t), axis=1)
        W.append(w)

    W = np.array(W)
    return A, k, W


#返回从真实世界坐标映射的图像坐标
def get_single_project_coor(A, W, k, coor):
    single_coor = np.array([coor[0], coor[1], coor[2], 1])

    #'''
    coor_norm = np.dot(W, single_coor)
    coor_norm /= coor_norm[-1]

    #r = np.linalg.norm(coor)
    r = np.linalg.norm(coor_norm)

    uv = np.dot(np.dot(A, W), single_coor)
    uv /= uv[-1]

    #畸变
    u0 = uv[0]
    v0 = uv[1]

    uc = A[0, 2]
    vc = A[1, 2]

    #u = (uc * r**2 * k[0] + uc * r**4 * k[1] - u0) / (r**2 * k[0] + r**4 * k[1] - 1)
    #v = (vc * r**2 * k[0] + vc * r**4 * k[1] - v0) / (r**2 * k[0] + r**4 * k[1] - 1)
    u = u0 + (u0 - uc) * r**2 * k[0] + (u0 - uc) * r**4 * k[1]
    v = v0 + (v0 - vc) * r**2 * k[0] + (v0 - vc) * r**4 * k[1]
    '''
    uv = np.dot(W, single_coor)
    uv /= uv[-1]
    # 透镜矫正
    x0 = uv[0]
    y0 = uv[1]
    r = np.linalg.norm(np.array([x0, y0]))

    k0 = 0
    k1 = 0

    x = x0 * (1 + r ** 2 * k0 + r ** 4 * k1)
    y = y0 * (1 + r ** 2 * k0 + r ** 4 * k1)

    #u = A[0, 0] * x + A[0, 2]
    #v = A[1, 1] * y + A[1, 2]
    [u, v, _] = np.dot(A, np.array([x, y, 1]))
    '''

    return np.array([u, v])


#返回所有点的真实世界坐标映射到的图像坐标与真实图像坐标的残差
def value(P, org_W, X, Y_real):
    M = (len(P) - 7) // 6
    N = len(X[0])
    A = np.array([
        [P[0], P[2], P[3]],
        [0, P[1], P[4]],
        [0, 0, 1]
    ])
    Y = np.array([])

    for i in range(M):
        m = 7 + 6 * i

        #取出当前图像对应的外参
        w = P[m:m + 6]

        # 不用旋转矩阵的变换是因为会有精度损失
        '''
        R = to_rotation_matrix(w[:3])
        t = w[3:].reshape(3, 1)
        W = np.concatenate((R, t), axis=1)
        '''
        W = org_W[i]
        #计算每幅图的坐标残差
        for j in range(N):
            Y = np.append(Y, get_single_project_coor(A, W, np.array([P[5], P[6]]), (X[i])[j]))

    error_Y  =  np.array(Y_real).reshape(-1) - Y

    return error_Y


#计算对应jacobian矩阵
def jacobian(P, WW, X, Y_real):
    M = (len(P) - 7) // 6
    N = len(X[0])
    K = len(P)
    A = np.array([
        [P[0], P[2], P[3]],
        [0, P[1], P[4]],
        [0, 0, 1]
    ])

    res = np.array([])

    for i in range(M):
        m = 7 + 6 * i

        w = P[m:m + 6]
        R = to_rotation_matrix(w[:3])
        t = w[3:].reshape(3, 1)
        W = np.concatenate((R, t), axis=1)

        for j in range(N):
            res = np.append(res, get_single_project_coor(A, W, np.array([P[5], P[6]]), (X[i])[j]))

    #求得x, y方向对P[k]的偏导
    J = np.zeros((K, 2 * M * N))
    for k in range(K):
        J[k] = np.gradient(res, P[k])

    return J.T


#将旋转矩阵分解为一个向量并返回，Rodrigues旋转向量与矩阵的变换,最后计算坐标时并未用到，因为会有精度损失
def to_rodrigues_vector(R):
    p = 0.5 * np.array([[R[2, 1] - R[1, 2]],
                        [R[0, 2] - R[2, 0]],
                        [R[1, 0] - R[0, 1]]])
    c = 0.5 * (np.trace(R) - 1)

    if np.linalg.norm(p) == 0:
        if c == 1:
            zrou = np.array([0, 0, 0])
        elif c == -1:
            R_plus = R + np.eye(3, dtype='float')

            norm_array = np.array([np.linalg.norm(R_plus[:, 0]),
                                   np.linalg.norm(R_plus[:, 1]),
                                   np.linalg.norm(R_plus[:, 2])])
            v = R_plus[:, np.where(norm_array == max(norm_array))]
            u = v / np.linalg.norm(v)
            if u[0] < 0 or (u[0] == 0 and u[1] < 0) or (u[0] == u[1] and u[0] == 0 and u[2] < 0):
                u = -u
            zrou = math.pi * u
        else:
            zrou = []
    else:
        u = p / np.linalg.norm(p)
        theata = math.atan2(np.linalg.norm(p), c)
        zrou = theata * u

    return zrou


#把旋转矩阵的一维向量形式还原为旋转矩阵并返回
def to_rotation_matrix(zrou):
    theta = np.linalg.norm(zrou)
    zrou_prime = zrou / theta

    W = np.array([[0, -zrou_prime[2], zrou_prime[1]],
                  [zrou_prime[2], 0, -zrou_prime[0]],
                  [-zrou_prime[1], zrou_prime[0], 0]])
    R = np.eye(3, dtype='float') + W * math.sin(theta) + np.dot(W, W) * (1 - math.cos(theta))

    return R

函数调用如下：

if __name__ == "__main__":
    cam = CameraZed2(resolution='1080', fps=30)
    # cam.grab_imgs()  # 获取标定图像
    cam.cal_exe()  # 进行相机标定

运行结果如下：

 运行结果中会显示相机外部参数

C:\Users\Lss\miniconda3\envs\python39gpu\python.exe C:/Users/Lss/PycharmProjects/zed_test/5.获取棋盘格照片.py
[2024-09-16 07:54:03 UTC][ZED][INFO] Logging level INFO
[2024-09-16 07:54:04 UTC][ZED][INFO] [Init]  Depth mode: ULTRA
[2024-09-16 07:54:05 UTC][ZED][INFO] [Init]  Camera successfully opened.
[2024-09-16 07:54:05 UTC][ZED][INFO] [Init]  Camera FW version: 1523
[2024-09-16 07:54:05 UTC][ZED][INFO] [Init]  Video mode: HD1080@30
[2024-09-16 07:54:05 UTC][ZED][INFO] [Init]  Serial Number: S/N 27513687
total max error:	 3.611829743106155
Homographic矩阵：
 [[-1.91815904e+01 -9.12404573e+01  1.60393200e+03  7.66795073e+01
  -1.14359437e+01  2.48346999e+02 -1.34722901e-02 -7.16791291e-03
   1.00000000e+00]
 [-3.79090210e+00 -1.04505852e+02  1.74620938e+03  8.68140791e+01
  -1.05677677e+01  2.23504218e+02 -4.10721627e-03 -1.86184235e-02
   1.00000000e+00]
 [ 2.96400861e+00 -1.12129589e+02  1.67718497e+03  8.89693955e+01
  -1.43052870e+01  2.22884570e+02  7.78437506e-04 -3.20383713e-02
   1.00000000e+00]
 [-8.12280248e-01 -8.62807053e+01  1.43191333e+03  6.81697578e+01
  -1.19266984e+01  2.73763341e+02 -1.42155717e-03 -2.26933469e-02
   1.00000000e+00]
 [-7.16926225e+00 -8.07128880e+01  1.66860126e+03  7.66698783e+01
  -2.98541859e+00  2.38760001e+02 -5.18820912e-03 -1.31034804e-03
   1.00000000e+00]
 [-6.81116850e+00 -9.75327792e+01  1.78208166e+03  8.89817877e+01
  -5.39530939e+00  2.27849888e+02 -5.28321377e-03 -6.20569301e-03
   1.00000000e+00]
 [ 8.10192808e+00 -8.54150071e+01  1.46299508e+03  6.41164677e+01
  -1.89410778e+00  2.19063629e+02 -5.51268022e-03 -2.35038759e-02
   1.00000000e+00]
 [-6.05563904e+00 -9.51566033e+01  1.84672706e+03  1.01001025e+02
  -9.51291432e+00  2.29020078e+02  5.20541902e-03  3.39810898e-03
   1.00000000e+00]
 [-2.83751860e+01 -8.52956587e+01  1.55992033e+03  5.40898763e+01
  -9.89255214e+00  2.91386570e+02 -2.98365350e-02 -9.95035436e-03
   1.00000000e+00]
 [ 1.68313291e+01 -1.02901098e+02  1.52228857e+03  9.33700096e+01
  -1.41296666e+01  2.59748141e+02  3.01243564e-02 -2.26778656e-02
   1.00000000e+00]
 [ 2.26445875e+01 -1.08908114e+02  1.89766433e+03  1.01711525e+02
  -1.15537000e+01  2.92754403e+02  3.35408373e-02 -2.05160237e-02
   1.00000000e+00]
 [ 4.87542938e+00 -8.35040139e+01  1.55835471e+03  7.47272219e+01
  -1.09712572e+01  2.84934447e+02  1.06896406e-02 -1.60474401e-02
   1.00000000e+00]
 [-6.75069327e+00 -9.49692960e+01  1.66816324e+03  7.56653267e+01
  -1.37133984e+01  3.25871027e+02 -5.27346855e-03 -2.03174083e-02
   1.00000000e+00]
 [ 1.17833571e+00 -9.21122312e+01  1.68283708e+03  7.36122803e+01
  -1.89096520e+00  2.06400355e+02 -7.91559286e-03 -1.63451515e-02
   1.00000000e+00]
 [-1.48496813e+00 -1.12615726e+02  1.89403639e+03  1.01269938e+02
  -1.22876870e+01  2.46349430e+02  3.83799005e-03 -1.51438384e-02
   1.00000000e+00]
 [-3.92385844e+00 -8.68241409e+01  1.59626016e+03  7.72599120e+01
  -8.70092895e+00  2.29296718e+02 -8.13952465e-04 -1.03996102e-02
   1.00000000e+00]
 [ 2.91925862e+00 -8.64939149e+01  1.70425798e+03  7.24452465e+01
   3.33922909e+00  2.12697307e+02 -7.70679943e-03 -1.15418651e-02
   1.00000000e+00]
 [ 9.33422931e+00 -9.05301776e+01  1.57020104e+03  7.15247225e+01
   3.11647697e+00  2.14773628e+02 -5.01993888e-03 -1.90165758e-02
   1.00000000e+00]
 [-1.13052662e+01 -1.02988034e+02  1.59686324e+03  8.74754986e+01
  -1.49513675e+01  1.85165153e+02 -5.14275246e-03 -1.45966800e-02
   1.00000000e+00]
 [ 6.03093125e+00 -9.25862446e+01  1.59884700e+03  7.18350052e+01
   6.62274995e-01  1.88087450e+02 -8.23777884e-03 -1.93522815e-02
   1.00000000e+00]
 [-1.84698646e+01 -9.27797933e+01  1.53587670e+03  7.14244492e+01
  -1.04816059e+01  1.86838374e+02 -1.76148980e-02 -1.14527318e-02
   1.00000000e+00]
 [-2.31810477e+01 -7.07989825e+01  1.76480362e+03  8.10711599e+01
  -1.99256894e+00  2.18834385e+02 -1.09247078e-02  1.60925585e-02
   1.00000000e+00]
 [-3.12210199e+01 -7.43842863e+01  1.64828347e+03  5.83515485e+01
  -2.64278878e-01  2.64919422e+02 -2.80555413e-02  4.69896977e-03
   1.00000000e+00]
 [-2.89249970e+01 -8.10476694e+01  1.69820291e+03  7.09170036e+01
  -2.00183312e+01  3.16570343e+02 -1.19191566e-02 -3.32816604e-03
   1.00000000e+00]
 [ 4.39460440e-01 -9.18280817e+01  1.69292969e+03  7.40580219e+01
  -1.32914931e+00  2.08713192e+02 -8.33760244e-03 -1.49590054e-02
   1.00000000e+00]
 [-5.34997122e+00 -8.64694284e+01  1.67346368e+03  7.11265000e+01
  -2.23309741e+00  2.90489319e+02 -1.08200044e-02 -1.01231443e-02
   1.00000000e+00]
 [-3.94986921e+00 -9.05847606e+01  1.67199135e+03  7.18667327e+01
  -2.95694683e+00  2.29460596e+02 -1.08804667e-02 -1.41425941e-02
   1.00000000e+00]
 [ 3.84123782e+00 -9.05308949e+01  1.67832740e+03  7.40922238e+01
   2.44437382e-01  1.92099737e+02 -6.11283765e-03 -1.53013360e-02
   1.00000000e+00]
 [-2.43929349e+01 -9.34299976e+01  1.47378973e+03  7.15882772e+01
  -2.32053110e+01  2.74611551e+02 -1.35393561e-02 -1.65593049e-02
   1.00000000e+00]
 [-3.20595677e+01 -8.20920145e+01  1.62401118e+03  8.61098524e+01
  -1.86904882e+01  3.04073723e+02 -8.08645033e-03  8.88535309e-03
   1.00000000e+00]
 [-1.33983936e+01 -5.44742460e+01  1.83873444e+03  1.06957779e+02
   4.82297372e+00  2.21974369e+02  3.89723362e-03  4.22555448e-02
   1.00000000e+00]]
内参矩阵:
 [[ 1.06731415e+03 -1.06962990e-01  9.54842923e+02]
 [ 0.00000000e+00  1.06730291e+03  5.22104497e+02]
 [ 0.00000000e+00  0.00000000e+00  1.00000000e+00]]
畸变校正系数:
 [ 0.0001511  -0.00010888]
外参矩阵:
 [[[-7.46678999e-02 -9.91816965e-01 -1.03555843e-01  7.62039542e+00]
  [ 9.82805071e-01 -9.07776326e-02  1.60790593e-01 -3.21412054e+00]
  [-1.68875392e-01 -8.97693122e-02  9.81540917e-01  1.25309451e+01]]

 [[ 1.05391659e-03 -9.74712140e-01 -2.23461703e-01  8.88468675e+00]
  [ 9.98784070e-01 -9.98789385e-03  4.82765339e-02 -3.35255332e+00]
  [-4.92876355e-02 -2.23240869e-01  9.73516432e-01  1.19832101e+01]]

 [[ 2.46112356e-02 -9.21950273e-01 -3.86525524e-01  8.15288736e+00]
  [ 9.99654101e-01  2.62821697e-02  9.62081566e-04 -3.37739350e+00]
  [ 9.27173805e-03 -3.86415504e-01  9.22278208e-01  1.20469981e+01]]

 [[ 8.08204835e-03 -9.36301499e-01 -3.51104236e-01  6.92056393e+00]
  [ 9.99725605e-01 -1.54312923e-04  2.34241381e-02 -3.60275674e+00]
  [-2.19862355e-02 -3.51197210e-01  9.36043335e-01  1.54836710e+01]]

 [[-2.82921461e-02 -9.99408928e-01 -1.95281725e-02  8.96611610e+00]
  [ 9.97174413e-01 -2.95781667e-02  6.90530370e-02 -3.55950656e+00]
  [-6.95898292e-02 -1.75193354e-02  9.97421841e-01  1.34079601e+01]]

 [[-1.94016045e-02 -9.97116653e-01 -7.33618295e-02  8.99814623e+00]
  [ 9.97927183e-01 -2.38159297e-02  5.97841088e-02 -3.20085143e+00]
  [-6.13589106e-02 -7.20498562e-02  9.95511880e-01  1.16099389e+01]]

 [[ 1.95127646e-01 -9.18309436e-01 -3.44431388e-01  7.41028432e+00]
  [ 9.77018804e-01  1.51281804e-01  1.50160156e-01 -4.41949690e+00]
  [-8.57872868e-02 -3.65816340e-01  9.26724850e-01  1.55653655e+01]]

 [[-1.11184101e-01 -9.92881744e-01  4.27075946e-02  9.00357195e+00]
  [ 9.92216741e-01 -1.13329100e-01 -5.15989770e-02 -2.95881634e+00]
  [ 5.60716955e-02  3.66382045e-02  9.97754282e-01  1.07748931e+01]]

 [[ 2.24916205e-03 -9.88439065e-01 -1.51601965e-01  7.89871830e+00]
  [ 9.09544193e-01 -6.09841951e-02  4.11108610e-01 -3.01195307e+00]
  [-4.15601134e-01 -1.38813337e-01  8.98891848e-01  1.39333181e+01]]

 [[-1.40043384e-01 -9.57923503e-01 -2.50540243e-01  6.68507572e+00]
  [ 9.14690937e-01 -2.82775331e-02 -4.03163577e-01 -3.09099537e+00]
  [ 3.79115206e-01 -2.85627281e-01  8.80164028e-01  1.25746080e+01]]

 [[-1.02967711e-01 -9.71223682e-01 -2.14760819e-01  1.02507542e+01]
  [ 9.15509027e-01 -8.12145675e-03 -4.02215443e-01 -2.49367857e+00]
  [ 3.88896993e-01 -2.38030672e-01  8.90000297e-01  1.16045751e+01]]

 [[-7.64523482e-02 -9.69270229e-01 -2.33816726e-01  8.58645072e+00]
  [ 9.83807079e-01 -3.52054957e-02 -1.75739021e-01 -3.37450012e+00]
  [ 1.62106967e-01 -2.43466211e-01  9.56266456e-01  1.51857867e+01]]

 [[-2.08478882e-02 -9.60456614e-01 -2.77648084e-01  9.07048795e+00]
  [ 9.97225219e-01 -3.98273255e-02  6.28939271e-02 -2.49537426e+00]
  [-7.14648689e-02 -2.75566466e-01  9.58621873e-01  1.35722015e+01]]

 [[ 1.11073474e-01 -9.71395327e-01 -2.09889974e-01  9.25091097e+00]
  [ 9.88000018e-01  8.51252852e-02  1.28878431e-01 -4.01199458e+00]
  [-1.07324962e-01 -2.21686273e-01  9.69193762e-01  1.35633744e+01]]

 [[-5.10432471e-02 -9.85768841e-01 -1.60169844e-01  9.44032399e+00]
  [ 9.97845376e-01 -4.37199323e-02 -4.89200766e-02 -2.77186895e+00]
  [ 4.12212725e-02 -1.62321777e-01  9.85876487e-01  1.07284476e+01]]

 [[-4.05858053e-02 -9.88874733e-01 -1.43106798e-01  8.24894409e+00]
  [ 9.99113431e-01 -4.17684011e-02  5.26804697e-03 -3.76586534e+00]
  [-1.11867807e-02 -1.42766116e-01  9.89693231e-01  1.37268177e+01]]

 [[ 1.32452412e-01 -9.80644345e-01 -1.44197872e-01  9.65755053e+00]
  [ 9.85462936e-01  1.14670819e-01  1.25353119e-01 -3.98747084e+00]
  [-1.06391539e-01 -1.58704982e-01  9.81577083e-01  1.37548168e+01]]

 [[ 1.95185833e-01 -9.48745664e-01 -2.48564187e-01  8.13154509e+00]
  [ 9.78292623e-01  1.70348336e-01  1.18004186e-01 -4.06141755e+00]
  [-6.96134642e-02 -2.66201256e-01  9.61400467e-01  1.41045472e+01]]

 [[-7.01236032e-02 -9.81880872e-01 -1.76047248e-01  7.08955443e+00]
  [ 9.95699291e-01 -7.96070025e-02  4.73882499e-02 -3.72091129e+00]
  [-6.05442098e-02 -1.71967085e-01  9.83240418e-01  1.17865114e+01]]

 [[ 1.74799877e-01 -9.53820721e-01 -2.44276553e-01  8.26728829e+00]
  [ 9.78056540e-01  1.39644910e-01  1.54611459e-01 -4.28815277e+00]
  [-1.13359636e-01 -2.65942344e-01  9.57300508e-01  1.37021687e+01]]

 [[-1.92979918e-02 -9.87562459e-01 -1.56038385e-01  7.01678417e+00]
  [ 9.73708670e-01 -5.39943021e-02  2.21305313e-01 -4.04907763e+00]
  [-2.26978003e-01 -1.47665181e-01  9.62640110e-01  1.28900358e+01]]

 [[-1.44300902e-01 -9.73942912e-01  1.74964152e-01  9.15403463e+00]
  [ 9.80725767e-01 -1.17223975e-01  1.56318613e-01 -3.42767068e+00]
  [-1.31735411e-01  1.94148769e-01  9.72086435e-01  1.20630512e+01]]

 [[-5.56403803e-02 -9.97413555e-01  4.55010847e-02  8.77440503e+00]
  [ 9.23774569e-01 -3.41327682e-02  3.81412505e-01 -3.25442995e+00]
  [-3.78872924e-01  6.32546818e-02  9.23284437e-01  1.35056925e+01]]

 [[-2.19161472e-01 -9.72491860e-01 -7.89166140e-02  9.27720590e+00]
  [ 9.62678425e-01 -2.28695148e-01  1.44736932e-01 -2.56518460e+00]
  [-1.58803334e-01 -4.42505627e-02  9.86318097e-01  1.33205549e+01]]

 [[ 1.06268333e-01 -9.76112067e-01 -1.89505341e-01  9.29986033e+00]
  [ 9.88002695e-01  8.21771490e-02  1.30757754e-01 -3.94892608e+00]
  [-1.12061213e-01 -2.01127196e-01  9.73134182e-01  1.34486829e+01]]

 [[ 6.09558789e-02 -9.89456397e-01 -1.31378915e-01  9.23643881e+00]
  [ 9.87015244e-01  4.01548541e-02  1.55526512e-01 -2.97707943e+00]
  [-1.48611201e-01 -1.39153247e-01  9.79056221e-01  1.37186412e+01]]

 [[ 8.36489676e-02 -9.79964281e-01 -1.80756350e-01  9.11465701e+00]
  [ 9.85509438e-01  5.44940347e-02  1.60628600e-01 -3.71957309e+00]
  [-1.47560148e-01 -1.91573506e-01  9.70322418e-01  1.35656720e+01]]

 [[ 1.24088277e-01 -9.72338387e-01 -1.97889264e-01  9.25580284e+00]
  [ 9.88751795e-01  1.04382462e-01  1.07117644e-01 -4.22210968e+00]
  [-8.34984287e-02 -2.08955409e-01  9.74353965e-01  1.36551665e+01]]

 [[-1.41027823e-01 -9.59207495e-01 -2.45014561e-01  6.42302011e+00]
  [ 9.73742653e-01 -1.79070448e-01  1.40566786e-01 -3.06340629e+00]
  [-1.78707582e-01 -2.18757300e-01  9.59275166e-01  1.32108106e+01]]

 [[-2.57852672e-01 -9.63338040e-01  7.41068123e-02  7.12244406e+00]
  [ 9.61797832e-01 -2.48625532e-01  1.14587412e-01 -2.32076023e+00]
  [-9.19615675e-02  1.00822442e-01  9.90645196e-01  1.13605712e+01]]

 [[-1.60688664e-01 -8.91129601e-01  4.24343243e-01  8.30765259e+00]
  [ 9.86231609e-01 -1.61983990e-01  3.32926514e-02 -2.82103393e+00]
  [ 3.90687444e-02  4.23850471e-01  9.04889171e-01  1.00319742e+01]]]

进程已结束,退出代码0