Opencv实验合集——实验九：姿势估计

在上一章节(相机校准)，你已经找到了相机矩阵，畸变系数等等参数。给出一个图案图像，我们便可以利用上面的信息用于计算其姿势，或者物体在空间中位于何处，比如如何旋转，如何移动等等问题。对于一个平面物体，我们可以假定 Z = 0，这样，问题现在便转化为了如何放置摄像机才能查看到我们的图案图像。所以如果我们知道物体在空间中的位置，我们便可以绘制一些 2D 图像用以模拟 3D 效果。

我们的问题是，我们想在我们棋盘的第一个角上绘制 3D 坐标系(x, y, z 坐标系)，其中 X 轴是蓝色，Y 轴是绿色，Z 轴是红色。所以从效果上讲，Z 轴应该感觉像是与棋盘垂直的。

1.定义

姿势估计是指通过分析图像或传感器数据来推断物体、人体或相机在三维空间中的姿势，即位置和方向。姿势通常由平移和旋转两个主要组成部分构成。

2.有关的函数方法

本节与上一个实验密切相关，重复的方法就不再一一赘述了

cv2.projectPoints 函数是 OpenCV 中用于将 3D 点投影到图像平面的函数。它是摄像机标定和相机投影中常用的一个函数。该函数的签名如下：

cv2.projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs[, imagePoints[, jacobian[, aspectRatio]]]) → imagePoints, jacobian

objectPoints: 三维物体点的坐标，是一个 numpy 数组，形状为 (N, 3)，N 是点的数量。
rvec: 旋转向量，描述了物体坐标系相对于相机坐标系的旋转。是一个长度为 3 的 numpy 数组。
tvec: 平移向量，描述了物体坐标系相对于相机坐标系的平移。是一个长度为 3 的 numpy 数组。
cameraMatrix: 相机内参矩阵，是一个 3x3 的矩阵。
distCoeffs: 相机的畸变系数，是一个包含畸变参数的 numpy 数组。
imagePoints (可选): 输出参数，是一个 numpy 数组，包含了三维点在图像平面上的投影坐标。
jacobian (可选): 输出参数，是一个 numpy 数组，包含了投影函数的导数信息。
aspectRatio (可选): 相机的纵横比，即图像的宽高比。

3.代码演示

首先我们先在棋盘上的三个点上建立三角坐标系XYZ，通过绘图进行展示，上个实验已经讲述了如何从世界坐标系转化相机坐标系，获得一系列参数（旋转，平移参数等），如何通过转化成相机坐标系再通过方法将3D点投影到图像平面上。

import numpy as np
import cv2 as cv
import glob

# 终止标准
criteria = (cv.TERM_CRITERIA_EPS + cv.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)
axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)
#axis是世界坐标系中的三个点，用来绘制图像上的相机坐标系

# 用于存储所有图像对象点与图像点的矩阵
objpoints = [] # 在真实世界中的 3d 点
imgpoints = [] # 在图像平面中的 2d 点

# images = glob.glob('*.jpg')
images = [r'C:\Users\xiaoou\Desktop\picture\chess.jpg']
def draw(img, corners, imgpts):#在一个角点上建立相机平面坐标系
    corner = tuple(corners[0].ravel())#在检测的众多角点中随机抽取一个
    img = cv.line(img, np.int32(corner), np.int32(tuple(imgpts[0].ravel())), (255,0,0), 5)
    img = cv.line(img, np.int32(corner), np.int32(tuple(imgpts[1].ravel())), (0,255,0), 5)
    img = cv.line(img, np.int32(corner), np.int32(tuple(imgpts[2].ravel())), (0,0,255), 5)
    return img

for fname in images:
    img = cv.imread(fname)
    gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)

    # 找到棋盘上所有的角点
    ret, corners = cv.findChessboardCorners(gray, (7,6), None)

    # 如果找到了，便添加对象点和图像点(在细化后)
    if ret == True:
        objpoints.append(objp)

        corners2 = cv.cornerSubPix(gray,corners, (11,11), (-1,-1), criteria)
        imgpoints.append(corners)
        # 绘制角点
        # cv.drawChessboardCorners(img, (7,6), corners2, ret)

        ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
        imgpts, jac = cv.projectPoints(axis,rvecs[0],tvecs[0],mtx,dist)
        img = draw(img, corners2, imgpts)

        cv.imshow('img', img)

        cv.waitKey(0)

现在把投影的点扩大为8个，构成一个长方体，绘制底部为绿色，顶部为红色，来让姿势估计更加具体。

import numpy as np
import cv2 as cv
import glob

# 终止标准
criteria = (cv.TERM_CRITERIA_EPS + cv.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)
axis = np.float32([ [0,0,0], [0,1,0], [1,1,0], [1,0,0],
                   [0,0,-1],[0,1,-1],[1,1,-1],[1,0,-1] ])

# 用于存储所有图像对象点与图像点的矩阵
objpoints = [] # 在真实世界中的 3d 点
imgpoints = [] # 在图像平面中的 2d 点

images = [r'C:\Users\xiaoou\Desktop\picture\chess.jpg']

def cube_draw(img, imgpts):#对8个平面的点建立相连建立一个长方体
    imgpts = np.int32(imgpts).reshape(-1,2)
    # 将底面绘制为绿色
    img = cv.drawContours(img, [imgpts[:4]], -1, (0,255,0), -3)
    # 将支柱绘制为蓝色
    for i,j in zip(range(4), range(4,8)):
        img = cv.line(img,tuple(imgpts[i]), tuple(imgpts[j]), (255,0,0), 3)
    # 将顶面绘制为红色
    img = cv.drawContours(img, [imgpts[4:]], -1, (0,0,255), 3)
    return img
for fname in images:
    img = cv.imread(fname)
    gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)

    # 找到棋盘上所有的角点
    ret, corners = cv.findChessboardCorners(gray, (7,6), None)

    # 如果找到了，便添加对象点和图像点(在细化后)
    if ret == True:
        objpoints.append(objp)

        corners2 = cv.cornerSubPix(gray,corners, (11,11), (-1,-1), criteria)
        imgpoints.append(corners)

        ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
        imgpts, jac = cv.projectPoints(axis,rvecs[0],tvecs[0],mtx,dist)
        img = cube_draw(img, imgpts)

        cv.imshow('img', img)

        cv.waitKey(0)