OpenCV-Python教程:47.姿态估计

基础

在上一节摄像机校准里，你找到了摄像机矩阵，畸变参数等，给一个模板图像，我们可以用上面的信息来计算它的姿态，或者物体是如何处于空间中的，比如如何旋转的，怎么被移动的。对于一个平面物体。我们可以假设Z = 0，这样，问题现在变成了摄像机如何放置的来看我们的模板图像，所以，如果我们知道物体是怎么放在空间中的，我们可以画出2D图来模拟3D效果。

我们的问题是，我们想画3D坐标系(x, y, z坐标轴）在我们的棋盘的第一个角，X轴是蓝色，Y轴是绿色，Z轴红色，所以Z轴应该感觉是核我们的棋盘面垂直的。

首先，让我们从上次标定的结果里加载摄像机矩阵和畸变参数。

import cv2
import numpy as np
import glob

# Load previously saved data
with np.load('B.npz') as X:
    mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]

现在让我们创建函数，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

然后和前面的例子一样，我们创建结束条件，物体点(棋盘角的3D点）和坐标轴点，坐标轴点是在3D空间画坐标轴的。我们画长度为3的坐标轴（单位和棋盘面积大小相关）。所以我们的X轴从(0,0,0)画到(3,0,0)，同理可得Y轴。对于Z轴，从（0，0,0）画到(0,0,-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)
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('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)

        # Find the rotation and translation vectors.
        rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, mtx, dist)

        # project 3D points to image plane
        imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)

        img = draw(img,corners2,imgpts)
        cv2.imshow('img',img)
        k = cv2.waitKey(0) & 0xff
        if k == 's':
            cv2.imwrite(fname[:6]+'.png', img)

cv2.destroyAllWindows()

看下面的结果，注意每个坐标轴都是3格长：

渲染一个立方体

如果你想要画䘝立方体，修改draw()函数和坐标轴点。

def draw(img, corners, imgpts):
    imgpts = np.int32(imgpts).reshape(-1,2)

    # draw ground floor in green
    img = cv2.drawContours(img, [imgpts[:4]],-1,(0,255,0),-3)

    # draw pillars in blue color
    for i,j in zip(range(4),range(4,8)):
        img = cv2.line(img, tuple(imgpts[i]), tuple(imgpts[j]),(255),3)

        # draw top layer in red color
        img = cv2.drawContours(img, [imgpts[4:]],-1,(0,0,255),3)

        return img

修改坐标轴点，他们是立方体在3D空间里的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]])

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