.. _pose_estimation:


姿勢推定
*********************

目的
==========

このチュートリアルでは,
    * 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個分の長さになっていることを確認してください:

    .. image:: images/pose_1.jpg
        :alt: Pose Estimation
        :align: center
        

箱を描画する
---------------

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


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

    .. image:: images/pose_2.jpg
        :alt: Pose Estimation
        :align: center
        

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


補足資料
===========================


課題
===========