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来渲染更复杂的图形。

推荐阅读更多精彩内容