姿勢推定¶
基礎¶
このチュートリアルは短いものになる．前のチュートリアルでカメラ・キャリブレーション(カメラ行列，レンズ歪み等)について学んだ．チェス盤のようなパターンが写った画像や空間におけるパターンの位置についての情報を与えると，そのパターンの姿勢を計算する事ができるようになった．平面物体に対し$Z=0$と仮定すると，そのパターンを観測するためにカメラが3次元空間中でどのような位置・姿勢になっていたかという問題に置き換わる．つまり、物体の空間における位置の情報があれば，3次元効果を与えるような2次元表示ができるようになる．それではどのように3次元効果を与えていくのか見ていこう．
チェスボードの最初の角に3次元座標系の座標軸を表示させることを目的としよう．X軸は青，Y軸は緑，Z軸は赤にする．また，Z軸はチェスボードに対して垂直に交わる軸であるとする．
まず初めに，カメラ行列と歪みパラメータのデータをファイルから読み込もう．ここで，キャリブレーションの結果が B.npz という名前のファイルに保存されていると仮定する(注: これはなんじゃ、という人は、前章「カメラ・キャリブレーション」を読み返そう):
import cv2
import numpy as np
import glob
# 前に記憶しておいたデータを読みだす
with np.load('B.npz') as X:
mtx, dist, _, _ = [X[i] for i in ('mtx','dist','rvecs','tvecs')]
print("mtx=",mtx,"\ndist=",dist)
次に，チェスボード上の制御点を( cv2.findChessboardCorners() 関数を使って)検出し，3次元座標を描画するための axis points を描画する draw という関数を作成しよう．
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
前の例と同様に，終了条件，3次元空間での点(チェスボード上の角の3次元点)，3次元軸を作成する．3次元軸とは軸を描くために必要な3次元空間中の点の集合である．ここでは長さが3(単位はチェスパターンの四角のサイズを基準にする)の軸を描画する．つまり，X軸は2点(0,0,0)，(3,0,0)を結び，Y軸は2点(0，0，0)，(0,3,0)を結ぶ直線になる．Z軸は(0,0,0)と(0,0,-3)を結ぶようにする(Z軸の負の方向は軸がカメラに向かって描かれることを意味する)．
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)
print(axis)
print(objp)
いつものように画像を読み込み，7x6の格子パターンを検出する．全ての格子パターンを検出した画像に対しては，サブピクセル精度での検出を行う．3次元空間中での回転・並進を計算するために cv2.solvePnPRansac() 関数を使う．この回転・並進のパラメータを計算すれば，3次元軸を画像中に投影する事ができる．単純に言うと 3次元空間中の点(3,0,0),(0,3,0),(0,0,3)に対応する2次元画像中の点を求める．これらの3次元軸の点を計算すれば， draw() 関数を使って最初の角から3次元軸を描画できる (注意: B.npzファイルと chessboards画像(zipファイル---展開必要)):
import cv2
import numpy as np
import glob
from matplotlib import pyplot as plt
# Load previously saved data
with np.load('B.npz') as X:
mtx, dist, _, _ = [X[i] for i in ('mtx','dist','rvecs','tvecs')]
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
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)
# i=0
for fname in glob.glob('chessboards/left*.jpg'):
# i += 1
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.
ret, rvecs, tvecs = cv2.solvePnP(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)
# plt.subplot(5,3,i),plt.imshow(img)
# plt.xticks([]),plt.yticks([])
k = cv2.waitKey(0) & 0xff
if k == 's':
cv2.imwrite(fname[:6]+'.png', img)
# plt.show()
cv2.destroyAllWindows()
# Notebookの場合はこれが必要
for _ in range(5):
cv2.waitKey(1)
以下に示す結果を見てみよう．各軸がチェスボードの四角形3個分の長さになっていることを確認せよ:
箱を描画する¶
3次元軸の代わりに箱を描画する場合，draw()関数の中身を以下のように変更する．.
修正版 draw() 関数:
def draw(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
修正版3次元軸は3次元空間中の箱の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] ])
import cv2
import numpy as np
import glob
from matplotlib import pyplot as plt
# Load previously saved data
with np.load('B.npz') as X:
mtx, dist, _, _ = [X[i] for i in ('mtx','dist','rvecs','tvecs')]
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
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([[0,0,0], [0,3,0], [3,3,0], [3,0,0],
[0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3] ])
# i=0
for fname in glob.glob('chessboards/left*.jpg'):
# i += 1
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.
ret, rvecs, tvecs = cv2.solvePnP(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)
# plt.subplot(5,3,i),plt.imshow(img)
# plt.xticks([]),plt.yticks([])
k = cv2.waitKey(0) & 0xff
if k == 's':
cv2.imwrite(fname[:6]+'.png', img)
# plt.show()
cv2.destroyAllWindows()
# Notebookの場合はこれが必要
for _ in range(5):
cv2.waitKey(1)
結果は以下のようになる:
CGやAugmented Realityに興味があれば、より複雑な物体を描画するためにOpenGLを使うとよいだろう．