cv2姿势估计

本篇博客主要介绍cv2中的姿态估计，在图像中绘制一些2D的线条来产生3D的效果。

下面是示例代码：

# encoding:utf-8
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


# 渲染一个立方体
def draw_cube(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 角点 和坐标轴点
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((6*7, 1, 3), np.float32)
objp[: ,: ,:2] = np.mgrid[0:7,  0:6].T.reshape(-1,1,2)
# 渲染一个立方体
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('../data/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 == ord('s'):
            cv2.imwrite(fname[:6] + '.png', img)
cv2.destroyAllWindows()

运行结果：

其中，获取摄像头矩阵和畸变参数文件B.npz的代码如下所示：

# encoding:utf-8
import numpy as np
import cv2
import glob

# 设置终止条件，迭代30次或移动0.001
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# 准备对象点，类似（0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((6 * 7, 3), np.float32)
objp[:, : 2] = np.mgrid[0: 7, 0: 6].T.reshape(-1, 2)  # np.mgrid()返回多维结构

# 从所有图像中存储对象点和图像点的数组
objpoints = []  # 真实世界的3D点
imgpoints = []  # 图像的2D点
# glob.globglob.glob函数的参数是字符串。这个字符串的书写和我们使用
# linux的shell命令相似，或者说基本一样。也就是说，只要我们按照平常使
# 用cd命令时的参数就能够找到我们所需要的文件的路径。字符串中可以包括“*”、
# “?”和"["、"]"，其中“*”表示匹配任意字符串，“?”匹配任意单个字符，
# [0-9]与[a-z]表示匹配0-9的单个数字与a-z的单个字符。
images = glob.glob('../data/left*.jpg')
images += glob.glob('../data/right*.jpg')

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # 找到棋盘边界，角点检测
    ret, corners = cv2.findChessboardCorners(gray, (7, 6), None)
    # 如果找到，则添加对象点和图像点
    if ret == True:
        objpoints.append(objp)
        # 亚像素级角点检测，在角点检测中精确化角点位置
        corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        imgpoints.append(corners)
        # 绘制并展示边界
#         cv2.drawChessboardCorners(img, (7, 6), corners2, ret)
#         print(objpoints)
#         print(imgpoints)
#         cv2.imshow('img', img)
#         cv2.waitKey(500)
# cv2.destroyAllWindows()
# 标定
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[:: -1], None, None)

np.savez('B.npz', mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs)

猜你喜欢