姿勢推定

目的

このチュートリアルでは,
  • calib3dモジュールを用いて、画像に3次元効果を作る方法を学ぶ.

基礎

このチュートリアルは短いものになる.前のチュートリアルでカメラ・キャリブレーション(カメラ行列,レンズ歪み等)について学んだ.チェス盤のようなパターンが写った画像や空間におけるパターンの位置についての情報を与えると,そのパターンの姿勢を計算する事ができるようんになった.平面物体に対し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')]

次に,チェスボード上の制御点を( 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次元軸を描画できる (コード, パラメタファイル, 画像(zipファイル---展開必要)):

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)

        # 回転行列と並進行列を計算する
        ret, rvecs, tvecs = cv2.solvePnP(objp, corners2, mtx, dist)

        # 3次元の点を画像平面に射影
        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)

    # 緑で底面を描画する
    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] ])

結果は以下のようになる: (コード, パラメタファイル, 画像(zipファイル---展開必要))

Pose Estimation

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

補足資料

課題