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

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

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

1. 效果圖

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

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

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

2. 原理

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

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

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

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

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

3. 源碼

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

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

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