姿勢推定

目的

このチュートリアルでは,
  • 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.
        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)
        k = cv2.waitKey(0) & 0xff
        if k == 's':
            cv2.imwrite(fname[:6]+'.png', img)

cv2.destroyAllWindows()

以下に示す結果を見てください.各軸がチェスボードの四角形3個分の長さになっていることを確認してください:

Pose Estimation

箱を描画する

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] ])

結果は以下のようになります:

Pose Estimation

CGやAugmented Realityに興味があればより複雑な物体を描画するためにOpenGLを使うといいでしょう.

補足資料

課題