姿勢推定¶
目的¶
- このチュートリアルでは,
- calib3d moduleを使って画像中に3次元効果を作る方法を学びます．
基礎¶
このチュートリアルは短いものになります．全チュートリアルでカメラキャリブレーション(カメラ行列，レンズ歪み等)について学びました．パターンが写った画像を与えると，もしくは空間中でパターンがどのように位置しているかという情報を与えると，パターンの姿勢を計算する事ができます．平面物体に対してZ=0と仮定すると，パターンを観測するためにカメラが3次元空間中でどのような位置・姿勢になるか知るという問題に置き換わります．物体が空間中のどの位置に存在するか分かれば，3次元効果を与えるような2次元表示ができるようになります．それではどのように3次元効果を与えていくのか見ていきましょう．
チェスボードの最初の角に3次元座標系の座標軸を表示させたることを目的とします．X軸は青，Y軸は緑，Z軸は赤にします．また，Z軸はチェスボードに対して垂直に交わる軸だとします．
まず初めに，カメラ行列と歪みパラメータのデータをファイルから読み込みましょう．ここで，キャリブレーションの結果が B.npz という名前のファイルに保存されていると仮定します．
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')]
次に，チェスボード上の制御点を検出( 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)
いつも通り各画像を読み込み，7x6の格子パターンを検出します．全ての格子パターンを検出した画像に対しては，サブピクセル精度での検出を行います．3次元空間中での回転・並進を計算するために cv2.solvePnPRansac() 関数を使います．この回転・並進のパラメータを計算すれば，3次元軸を画像中に投影する事が出来ます．単純に言うと 3次元空間中の点(3,0,0),(0,3,0),(0,0,3)に対応する2次元画像中の点を求めます．これらの3次元軸の点を計算すれば， draw() 関数を使って最初の角から3次元軸を描画できます．
for fname in glob.glob('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 == 's':
cv2.imwrite(fname[:6]+'.png', img)
cv2.destroyAllWindows()
以下に示す結果を見てください．各軸がチェスボードの四角形3個分の長さになっていることを確認してください:
箱を描画する¶
3次元軸の代わりに箱を描画する場合，draw()の中身を以下のように変更します．.
修正版 draw() 関数:
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
修正版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] ])
結果は以下のようになります:
CGやAugmented Realityに興味があればより複雑な物体を描画するためにOpenGLを使うといいでしょう．
