OpenCV中的姿勢(shì)估計(jì)及3D效果（3D坐標(biāo)軸，3D立方體）繪制

• • 1. 效果圖
  • 2. 原理
  • 3. 源碼
  • • 3.1 姿態(tài)估計(jì)后繪制3D坐標(biāo)軸
    • 3.2 姿態(tài)估計(jì)后繪制立方體
  • 參考

這篇博客將延續(xù)上一篇博客：OpenCV中的相機(jī)失真、內(nèi)外參、不失真圖像、相機(jī)校準(zhǔn) ， 學(xué)習(xí)如何利用 calib3d 模塊在圖像中創(chuàng)建一些 3D 效果（3D坐標(biāo)軸，3D立方體）。

1. 效果圖

根據(jù)相機(jī)矩陣、失真參數(shù)估計(jì)軌跡姿態(tài)后，繪制坐標(biāo)軸效果圖如下：
X軸綠色，Y軸藍(lán)色，Z軸紅色垂直于平面，軸長均為3個(gè)正方形；

根據(jù)相機(jī)矩陣、失真參數(shù)估計(jì)軌跡姿態(tài)后，繪制坐標(biāo)軸效果圖2如下：
X軸綠色，Y軸藍(lán)色，Z軸紅色垂直于平面，軸長均為3個(gè)正方形；

繪制立方體效果圖1如下：
綠色繪制底層，藍(lán)色繪制柱子，紅色繪制頂層；
繪制立方體效果圖2如下：
綠色繪制底層，藍(lán)色繪制柱子，紅色繪制頂層；

2. 原理

有了相機(jī)矩陣、失真系數(shù)等。給定一個(gè)模式圖像就可以利用上述信息來計(jì)算它的姿態(tài)。

目標(biāo)是在棋盤的第一個(gè)角上繪制 3D 坐標(biāo)軸（X、Y、Z 軸）。 X 軸為藍(lán)色，Y 軸為綠色，Z 軸為紅色，所以Z 軸應(yīng)該感覺像是垂直于棋盤平面。
或者繪制3D立方體，底層綠色，柱子紅色，頂層紅色；

1. 首先，從之前的校準(zhǔn)結(jié)果中加載相機(jī)矩陣和失真系數(shù)。
2. 然后計(jì)算姿態(tài)，旋轉(zhuǎn)和平移向量
3. 然后投影3D點(diǎn)到圖像平面
4. 繪制線、繪制立方體；

• cv2.findChessboardCorners() 尋找矩形的網(wǎng)格模式
• cv2.cornerSubPix() 角點(diǎn)像素精細(xì)化
• cv2.solvePnPRansac() 計(jì)算姿態(tài)，旋轉(zhuǎn)和平移向量
• cv2.projectPoints() 投影3D點(diǎn)到圖像平面

如果對(duì)圖形、增強(qiáng)現(xiàn)實(shí)等感興趣，可以使用 OpenGL 來渲染更復(fù)雜的圖形。

3. 源碼

3.1 姿態(tài)估計(jì)后繪制3D坐標(biāo)軸

# 有了相機(jī)矩陣、失真系數(shù)等。給定一個(gè)模式圖像就可以利用上述信息來計(jì)算它的姿態(tài)。
# 目標(biāo)是在棋盤的第一個(gè)角上繪制 3D 坐標(biāo)軸（X、Y、Z 軸）。 X 軸為藍(lán)色，Y 軸為綠色，Z 軸為紅色。所以Z 軸應(yīng)該感覺像是垂直于棋盤平面。# USAGE
# python pose_estimation_line.py
import cv2
import numpy as np
import glob# 構(gòu)建方法draw，它使用棋盤中的角點(diǎn)（使用 cv2.findChessboardCorners() 獲得）和軸點(diǎn)來繪制 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# 首先，從之前的校準(zhǔn)結(jié)果中加載相機(jī)矩陣、失真系數(shù)
with np.load('qpimgs/B.npz') as X:mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]# 創(chuàng)建終止條件、對(duì)象點(diǎn)（棋盤中角的 3D 點(diǎn)）和軸點(diǎn)。
# 軸點(diǎn)是 3D 空間中用于繪制軸的點(diǎn)，繪制長度為 3 的軸（單位將根據(jù)國際象棋平方大小，因?yàn)閷⒏鶕?jù)該大小進(jìn)行校準(zhǔn)）。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)# 坐標(biāo)軸的3個(gè)點(diǎn)，X 軸是從(0,0,0) 到 (3,0,0) 繪制的，Y 軸是從(0,0,0) 到 (0,3,0)，Z 軸從(0,0,0) 到 (0,0,-3) 繪制。負(fù)數(shù)表示它被拉向相機(jī)。
axis = np.float32([[3, 0, 0], [0, 3, 0], [0, 0, -3]]).reshape(-1, 3)# 加載每個(gè)圖像。搜索 7x6 網(wǎng)格模式。如果找到，我們用像素角點(diǎn)精細(xì)化它。然后使用函數(shù)cv2.solvePnPRansac() 計(jì)算旋轉(zhuǎn)和平移矩陣。有了變換矩陣，就可以使用它們將軸點(diǎn)投影到圖像平面。
# 簡(jiǎn)單來說，在3D 空間中找到與 (3,0,0),(0,3,0),(0,0,3) 中的每一個(gè)對(duì)應(yīng)的圖像平面上的點(diǎn)。獲得它們后，使用 draw() 函數(shù)從第一個(gè)角到這些點(diǎn)中的每一個(gè)繪制線。完畢 ！！！
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)# 計(jì)算旋轉(zhuǎn)和平移向量# 用于估計(jì)最小采樣集步驟的攝影機(jī)姿勢(shì)的默認(rèn)方法bool, rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, mtx, dist)# 投影3D點(diǎn)到圖像平面imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)# 繪制角到軸點(diǎn)為線，每個(gè)線長為3個(gè)正方形img = draw(img, corners2, imgpts)cv2.imshow('drawLine res', img)k = cv2.waitKey(0) & 0xffif k == 's':print(str(fname[:6]))cv2.imwrite(fname[:6] + '.png', img)cv2.destroyAllWindows()

3.2 姿態(tài)估計(jì)后繪制立方體

# 有了相機(jī)矩陣、失真系數(shù)等。給定一個(gè)模式圖像就可以利用上述信息來計(jì)算它的姿態(tài)。
# 目標(biāo)是在棋盤的第一個(gè)角上繪制立方體，其中綠色繪制底層，藍(lán)色繪制柱子，紅色繪制頂層；# 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)# 用藍(lán)色繪制柱子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# 首先，從之前的校準(zhǔn)結(jié)果中加載相機(jī)矩陣、失真系數(shù)
with np.load('qpimgs/B.npz') as X:mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]# 創(chuàng)建終止條件、對(duì)象點(diǎn)（棋盤中角的 3D 點(diǎn)）和軸點(diǎn)。
# 軸點(diǎn)是 3D 空間中用于繪制軸的點(diǎn)，繪制長度為 3 的軸（單位將根據(jù)國際象棋平方大小，因?yàn)閷⒏鶕?jù)該大小進(jìn)行校準(zhǔn)）。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個(gè)頂點(diǎn)
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]])# 加載每個(gè)圖像。搜索 7x6 網(wǎng)格模式。如果找到，我們用像素角點(diǎn)精細(xì)化它。然后使用函數(shù)cv2.solvePnPRansac() 計(jì)算旋轉(zhuǎn)和平移矩陣。有了變換矩陣，就可以使用它們將軸點(diǎn)投影到圖像平面。
# 簡(jiǎn)單來說，在3D 空間中找到與 (3,0,0),(0,3,0),(0,0,3) 中的每一個(gè)對(duì)應(yīng)的圖像平面上的點(diǎn)。獲得它們后，使用 draw() 函數(shù)從第一個(gè)角到這些點(diǎn)中的每一個(gè)繪制線。完畢 ！！！
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)# 計(jì)算旋轉(zhuǎn)和平移向量# 用于估計(jì)最小采樣集步驟的攝影機(jī)姿勢(shì)的默認(rèn)方法bool, rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, mtx, dist)# 投影3D點(diǎn)到圖像平面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) & 0xffif 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

總結(jié)

以上是生活随笔為你收集整理的OpenCV中的姿势估计及3D效果（3D坐标轴，3D立方体）绘制的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。