OpenCV中的姿势估计及3D效果（3D坐标轴，3D立方体）绘制

Posted 2021-08-29 程序媛一枚~

OpenCV中的姿势估计及3D效果（3D坐标轴，3D立方体）绘制

• • 1. 效果图
  • 2. 原理
  • 3. 源码
  • • 3.1 姿态估计后绘制3D坐标轴
    • 3.2 姿态估计后绘制立方体
  • 参考

这篇博客将延续上一篇博客：OpenCV中的相机失真、内外参、不失真图像、相机校准 ， 学习如何利用 calib3d 模块在图像中创建一些 3D 效果（3D坐标轴，3D立方体）。

1. 效果图

根据相机矩阵、失真参数估计轨迹姿态后，绘制坐标轴效果图如下：
X轴绿色，Y轴蓝色，Z轴红色垂直于平面，轴长均为3个正方形；

根据相机矩阵、失真参数估计轨迹姿态后，绘制坐标轴效果图2如下：
X轴绿色，Y轴蓝色，Z轴红色垂直于平面，轴长均为3个正方形；

绘制立方体效果图1如下：
绿色绘制底层，蓝色绘制柱子，红色绘制顶层；
绘制立方体效果图2如下：
绿色绘制底层，蓝色绘制柱子，红色绘制顶层；

2. 原理

有了相机矩阵、失真系数等。给定一个模式图像就可以利用上述信息来计算它的姿态。

目标是在棋盘的第一个角上绘制 3D 坐标轴（X、Y、Z 轴）。 X 轴为蓝色，Y 轴为绿色，Z 轴为红色，所以Z 轴应该感觉像是垂直于棋盘平面。
或者绘制3D立方体，底层绿色，柱子红色，顶层红色；

1. 首先，从之前的校准结果中加载相机矩阵和失真系数。
2. 然后计算姿态，旋转和平移向量
3. 然后投影3D点到图像平面
4. 绘制线、绘制立方体；

• cv2.findChessboardCorners() 寻找矩形的网格模式
• cv2.cornerSubPix() 角点像素精细化
• cv2.solvePnPRansac() 计算姿态，旋转和平移向量
• cv2.projectPoints() 投影3D点到图像平面

如果对图形、增强现实等感兴趣，可以使用 OpenGL 来渲染更复杂的图形。

3. 源码

3.1 姿态估计后绘制3D坐标轴

# 有了相机矩阵、失真系数等。给定一个模式图像就可以利用上述信息来计算它的姿态。
# 目标是在棋盘的第一个角上绘制 3D 坐标轴（X、Y、Z 轴）。 X 轴为蓝色，Y 轴为绿色，Z 轴为红色。所以Z 轴应该感觉像是垂直于棋盘平面。

# USAGE
# python pose_estimation_line.py
import cv2
import numpy as np
import glob


# 构建方法draw，它使用棋盘中的角点（使用 cv2.findChessboardCorners() 获得）和轴点来绘制 3D 轴。
def draw(img, corners, imgpts):
    corner = tuple(corners[0].ravel())
    img = cv2.line(img, corner, tuple(imgpts[0].ravel()), (255, 0, 0), 5)
    img = cv2.line(img, corner, tuple(imgpts[1].ravel()), (0, 255, 0), 5)
    img = cv2.line(img, corner, tuple(imgpts[2].ravel()), (0, 0, 255), 5)
    return img


# 首先，从之前的校准结果中加载相机矩阵、失真系数
with np.load('qpimgs/B.npz') as X:
    mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]

# 创建终止条件、对象点（棋盘中角的 3D 点）和轴点。
# 轴点是 3D 空间中用于绘制轴的点，绘制长度为 3 的轴（单位将根据国际象棋平方大小，因为将根据该大小进行校准）。

criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((6 * 7, 3), np.float32)
objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2)

# 坐标轴的3个点，X 轴是从(0,0,0) 到 (3,0,0) 绘制的，Y 轴是从(0,0,0) 到 (0,3,0)，Z 轴从(0,0,0) 到 (0,0,-3) 绘制。负数表示它被拉向相机。
axis = np.float32([[3, 0, 0], [0, 3, 0], [0, 0, -3]]).reshape(-1, 3)

# 加载每个图像。搜索 7x6 网格模式。如果找到，我们用像素角点精细化它。然后使用函数cv2.solvePnPRansac() 计算旋转和平移矩阵。有了变换矩阵，就可以使用它们将轴点投影到图像平面。
# 简单来说，在3D 空间中找到与 (3,0,0),(0,3,0),(0,0,3) 中的每一个对应的图像平面上的点。获得它们后，使用 draw() 函数从第一个角到这些点中的每一个绘制线。完毕 ！！！
for fname in glob.glob('qpimgs/left*.jpg'):
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, (7, 6), None)

    if ret == True:
        corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)

        # 计算旋转和平移向量
        # 用于估计最小采样集步骤的摄影机姿势的默认方法
        bool, rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, mtx, dist)

        # 投影3D点到图像平面
        imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)

        # 绘制角到轴点为线，每个线长为3个正方形
        img = draw(img, corners2, imgpts)
        cv2.imshow('drawLine res', img)
        k = cv2.waitKey(0) & 0xff
        if k == 's':
            print(str(fname[:6]))
            cv2.imwrite(fname[:6] + '.png', img)

cv2.destroyAllWindows()

3.2 姿态估计后绘制立方体

# 有了相机矩阵、失真系数等。给定一个模式图像就可以利用上述信息来计算它的姿态。
# 目标是在棋盘的第一个角上绘制立方体，其中绿色绘制底层，蓝色绘制柱子，红色绘制顶层；

# USAGE
# python pose_estimation_cube.py
import cv2
import numpy as np
import glob


# 绘制立方体
def drawCube(img, corners, imgpts):
    imgpts = np.int32(imgpts).reshape(-1, 2)

    # 用绿色绘制底层
    img = cv2.drawContours(img, [imgpts[:4]], -1, (0, 255, 0), -3)

    # 用蓝色绘制柱子
    for i, j in zip(range(4), range(4, 8)):
        img = cv2.line(img, tuple(imgpts[i]), tuple(imgpts[j]), (255), 3)

    # 用红色绘制顶层
    img = cv2.drawContours(img, [imgpts[4:]], -1, (0, 0, 255), 3)

    return img


# 首先，从之前的校准结果中加载相机矩阵、失真系数
with np.load('qpimgs/B.npz') as X:
    mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]

# 创建终止条件、对象点（棋盘中角的 3D 点）和轴点。
# 轴点是 3D 空间中用于绘制轴的点，绘制长度为 3 的轴（单位将根据国际象棋平方大小，因为将根据该大小进行校准）。

criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((6 * 7, 3), np.float32)
objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2)

# 立方体的8个顶点
axis = np.float32([[0, 0, 0], [0, 3, 0], [3, 3, 0], [3, 0, 0],
                   [0, 0, -3], [0, 3, -3], [3, 3, -3], [3, 0, -3]])

# 加载每个图像。搜索 7x6 网格模式。如果找到，我们用像素角点精细化它。然后使用函数cv2.solvePnPRansac() 计算旋转和平移矩阵。有了变换矩阵，就可以使用它们将轴点投影到图像平面。
# 简单来说，在3D 空间中找到与 (3,0,0),(0,3,0),(0,0,3) 中的每一个对应的图像平面上的点。获得它们后，使用 draw() 函数从第一个角到这些点中的每一个绘制线。完毕 ！！！
for fname in glob.glob('qpimgs/left*.jpg'):
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, (7, 6), None)

    if ret == True:
        corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)

        # 计算旋转和平移向量
        # 用于估计最小采样集步骤的摄影机姿势的默认方法
        bool, rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, mtx, dist)

        # 投影3D点到图像平面
        imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)

        # 绘制立方体
        cube_img = drawCube(img, corners2, imgpts)
        cv2.imshow('drawCube res', cube_img)
        k = cv2.waitKey(0) & 0xff
        if k == 's':
            print(str(fname[:6]))
            cv2.imwrite(fname[:6] + '.png', img)

cv2.destroyAllWindows()

参考

• https://docs.opencv.org/3.0-beta/doc/py_tutorials/py_calib3d/py_pose/py_pose.html#pose-estimation