【OpenCV】OpenCV-Python实现相机标定+利用棋盘格相对位姿估计

写在前面：

    这次要实现的功能：实时检测棋盘格相对于摄像头的距离以及位姿。为此主要步骤可分为以下三个步骤：标定图片的拍摄、相机的标定、以及棋盘格位姿的实时解算。

---

目录

1. 标定图片的拍摄

2. 相机的标定

3. 棋盘格位姿的实时解算

4. 需要注意的点

5. 运行效率问题

---

1. 标定图片的拍摄

棋盘格图片

        打印上面的图片，尽量铺满一张A4纸，边缘留出一定的空白以方便握持。这里使用的是10×7的棋盘格，内部有9×6个角点。

        然后使用摄像头来随意拍摄棋盘格的15-20张照片。这里笔者踩了一个小坑，笔者一开始用的是电脑的相机应用拍摄了20张照片，分辨率为1280×720。进行后面的步骤都没什么问题，但是测得的结果怎么都不准。这是因为这颗摄像头录像和拍照的分辨率不一致，因此编写一个小程序来获得标定所用的照片。

import cv2
camera = cv2.VideoCapture(0)
i = 1
while i < 50:
    _, frame = camera.read()
    cv2.imwrite("E:/images/"+str(i)+'.png', frame, [int(cv2.IMWRITE_PNG_COMPRESSION), 0]) 
    cv2.imshow('frame', frame)
    i += 1
    if cv2.waitKey(200) & 0xFF == 27: # 按ESC键退出
        break
cv2.destroyAllWindows()

        保存的图片经过手动筛选，挑选出清晰度和完整度较好的20张，如下所示：

利用摄像头拍摄的20张棋盘格图片

2. 相机的标定

        相机标定的原理此处略去，网上可以找到大量的相关资料。

        有一点需要注意，标定中将世界坐标系的建在标定板上，所有的z坐标均为0。但是x和y坐标需要经过测量得出。笔者所打印的棋盘格一格的边长为2.6厘米，因此每一个角点在世界坐标系中的坐标都需要以2.6厘米为倍数。

        相机标定部分的主要代码如下：

objp = np.zeros((6 * 9, 3), np.float32)
objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2)  # 将世界坐标系建在标定板上，所有点的Z坐标全部为0，所以只需要赋值x和y
objp = 2.6 * objp   # 打印棋盘格一格的边长为2.6cm
obj_points = []     # 存储3D点
img_points = []     # 存储2D点
images=glob.glob("E:/image/*.png")  #黑白棋盘的图片路径

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    size = gray.shape[::-1]
    ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)
    if ret:
        obj_points.append(objp)
        corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001))  
        if [corners2]:
            img_points.append(corners2)
        else:
            img_points.append(corners)
        cv2.drawChessboardCorners(img, (9, 6), corners, ret)  # 记住，OpenCV的绘制函数一般无返回值
        cv2.waitKey(1)
_, mtx, dist, _, _ = cv2.calibrateCamera(obj_points, img_points, size, None, None)

# 内参数矩阵
Camera_intrinsic = {"mtx": mtx,"dist": dist,}

        后面解算位姿所需要的参数为内参数矩阵mtx和畸变系数dist。据此求得的内参数矩阵和畸变系数如下：

        内参矩阵的具体表达式如下：

        其中，和分别是每个像素在图像平面和方向上的物理尺寸，是图像坐标系原点在像素坐标系中的坐标，为摄像头的焦距，，为焦距与像素物理尺寸的比值，单位为个（像素数目）。 

        据此可以得到，这台摄像头的，说明焦距约等于450个像素的物理尺寸。，。这台摄像头的像素为640×480，因此的理论值应为320，的理论值应为240。误差主要是因为摄像头的分辨率太低，实际角点在像素坐标系中显示不准；此外，目标坐标系的测量时也会带来误差。

3. 棋盘格位姿的实时解算

        利用solvePnP函数可以实时解算出每一帧的旋转矢量rvec和平移矢量tvec。旋转矢量虽然简洁，但是作为结果显示不够直观，故需要将其转换为欧拉角。

        在欧拉角中，俯仰角（pitch）代表绕x轴旋转的角度， 偏航角（yaw）代表绕y轴旋转的角度，滚转角（roll）代表绕z轴旋转的角度。其中，默认逆时针选择为正，顺时针旋转为负。

        该部分的主要代码如下：

obj_points = objp   # 存储3D点
img_points = []     # 存储2D点

#从摄像头获取视频图像
camera = cv2.VideoCapture(0)

while True:
    _, frame = camera.read()
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    size = gray.shape[::-1]
    ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)
    if ret:    # 画面中有棋盘格
        img_points = np.array(corners)
        cv2.drawChessboardCorners(frame, (9, 6), corners, ret)
        # rvec: 旋转向量 tvec: 平移向量
        _, rvec, tvec = cv2.solvePnP(obj_points, img_points, Camera_intrinsic["mtx"], Camera_intrinsic["dist"])    # 解算位姿
        distance = math.sqrt(tvec[0]**2+tvec[1]**2+tvec[2]**2)  # 计算距离
        rvec_matrix = cv2.Rodrigues(rvec)[0]    # 旋转向量->旋转矩阵
        proj_matrix = np.hstack((rvec_matrix, tvec))    # hstack: 水平合并
        eulerAngles = cv2.decomposeProjectionMatrix(proj_matrix)[6]  # 欧拉角
        pitch, yaw, roll = eulerAngles[0], eulerAngles[1], eulerAngles[2]
        cv2.putText(frame, "dist: %.2fcm, yaw: %.2f, pitch: %.2f, roll: %.2f" % (distance, yaw, pitch, roll), (10, frame.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
        cv2.imshow('frame', frame)
        if cv2.waitKey(1) & 0xFF == 27: # 按ESC键退出
            break
    else:   # 画面中没有棋盘格
        cv2.putText(frame, "Unable to Detect Chessboard", (20, frame.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX, 1.3, (0, 0, 255), 3) 
        cv2.imshow('frame', frame)
        if cv2.waitKey(1) & 0xFF == 27: # 按ESC键退出
            break
cv2.destroyAllWindows()

        当画面中检测不到棋盘格，或距离过远无法检测棋盘格的角点时，显示结果如下：

        当画面中能正常检测棋盘格的角点时，显示结果如下：

        左上角第一个红点为标定时所确定的世界坐标系的原点，沿棋盘格向右为x轴正方向，沿棋盘格向下为y轴正方向。 

        此时该棋盘格的坐标原点与摄像头的距离为43.18cm，偏航角为-1.3°，俯仰角为-26.48°，滚转角为3.92°。经过验证，该结果与实际的误差在1%以内，证明了结果的正确性。

4. 需要注意的点

        相对位姿估计的基本问题

• 输入：相机内参数；多个空间上的特征点在目标坐标系(3D)和相平面坐标系(2D)坐标
• 输出：目标坐标系相对相机坐标系的位置和姿态

        也就是说，solvePnP函数求解的是目标坐标系相对相机坐标系的位置和姿态。为了提高结果的可读性，最好将初始位置的目标坐标系与相机坐标系的方向同一化。

        相机坐标系的x轴和y轴对应着相平面坐标系的u轴和v轴，因此在实际操作中，确定目标坐标系时按照像素坐标系的方向来确定即可。具体如下：

        此处目标坐标系的坐标原点确定在第一个角点处，目的是为了在编程中简化目标坐标系的设置。实际上将目标坐标系的坐标原点确定在棋盘格的中心更为合理。

        为了验证输出结果为目标坐标系相对于相机坐标系的位姿（顺序很重要），将棋盘格绕x轴逆时针旋转45°，输出俯仰角pitch也约为45°；将棋盘格绕y轴逆时针旋转45°，输出偏航角yaw也约为45°，将棋盘格绕z轴逆时针旋转45°，输出滚转角roll也约为45°。由此证明了输出结果为目标坐标系相对于相机坐标系的位姿。（如果结果符号不对，说明在编程中目标坐标系的设置有误）

5. 运行效率问题

        在程序中加入时钟检测代码，可以得到每运行一帧所需要的计算时间。

逐帧运行时间

        该程序运行于i7-8550U低压CPU平台，性能没有很高。当正常解算位姿时，一帧的平均运行时间在0.01秒以内，可以支持60Hz或90Hz的摄像头运行。但是当画面中没有棋盘格时，一帧的平均运行时间更长，原因是findChessboardCorners在寻找不到角点的情况下运算量更大。但是我们关注的只是正常解算位姿时的运行效率，无法检测棋盘格时的运行效率可以不予考虑。