OpenCV3 を用いて物体の位置姿勢を推定
[History] [Last Modified] (2020/02/17 15:21:17)
ここは
趣味のプログラミングを楽しむための情報共有サービス。記事の一部は有料設定にして公開できます。 詳しくはこちらをクリック📝
Recent posts
Popular pages

概要

ワールド座標に固定された単一カメラが存在するとします。RGB-D カメラではなく RGB カメラです。この単一カメラからはカラーまたはグレースケール画像が取得できます。カメラキャリブレーションの考え方を利用すると、カメラで取得した画像に写っている既知の物体のワールド座標における位置姿勢を推定できます。

カメラキャリブレーションによる内部パラメータ推定結果の保存

単一カメラの内部パラメータを再利用できるように、何らかの形式で保存しておきます。

err, KK, distCoeffs, rvecs, tvecs = cv.calibrateCamera(objpoints, imgpoints, imageSize, None, None)
np.savez_compressed('calib', KK=KK, distCoeffs=distCoeffs)

生成されたファイル

$ file calib.npz
calib.npz: Zip archive data, at least v2.0 to extract

物体の位置姿勢を推定

内部パラメータと歪み係数が分かっている場合は cv.calibrateCamera ではなく cv.solvePnP を用いて外部パラメータだけを推定します。推定結果を用いて、cv.solvePnP の第一引数として与えた objp と同じ座標系の各点を画像に投影するためには cv.projectPoints を用います。

Uploaded Image

#!/usr/bin/python
# -*- coding: utf-8 -*-
import numpy as np
import cv2 as cv
import glob

def Main():

    # cornerSubPix の閾値
    criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)

    # ワールド座標系におけるキャリブレーションボードの各点の座標
    # (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
    objp = np.zeros((6*7,3), np.float32)
    objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)

    # 推定した位置姿勢を分かりやすく可視化するための仮想的な物体
    cube = 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]])

    # キャリブレーションで推定した単一カメラの内部パラメータ
    with np.load('calib.npz') as data:
        KK, distCoeffs = [data[i] for i in ('KK', 'distCoeffs')]

    # 画像に写っている物体の位置姿勢を推定
    for fname in glob.glob('left*.jpg'):

        img = cv.imread(fname)
        gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)

        # キャリブレーションボードの位置姿勢を推定してみます
        ret, corners = cv.findChessboardCorners(gray, (7,6), None)

        if ret == True:
            # 座標の精度を上げる
            corners2 = cv.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)

            # solvePnP を用いて外部パラメータ (物体の位置姿勢に相当) だけを推定します。
            err, rvecs, tvecs = cv.solvePnP(objp, corners2, KK, distCoeffs)

            # 推定結果を可視化するために、物体 cube を画像に投影してみます。
            imgpts, jac = cv.projectPoints(cube, rvecs, tvecs, KK, distCoeffs)
            img = draw(img, imgpts)
            cv.imshow('img', img)
            cv.waitKey(0)
    cv.destroyAllWindows()

def draw(img, imgpts):
    imgpts = np.int32(imgpts).reshape(-1, 2)
    # draw ground floor in green
    img = cv.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 = cv.line(img, tuple(imgpts[i]), tuple(imgpts[j]), (255), 3)
    # draw top layer in red color
    img = cv.drawContours(img, [imgpts[4:]], -1, (0, 0, 255), 3)
    return img

if __name__ == '__main__':
    Main()

画像の挿入

内部パラメータ $K$ と外部パラメータ $T'$ によるカメラキャリブレーションの式を、透視変換 (ホモグラフィ; Homography) として捉えると、例えば推定した位置に画像を挿入することができます。

$$s \begin{pmatrix} u \\ v \\ 1 \end{pmatrix} = K T' \begin{pmatrix} x \\ y \\ 1 \end{pmatrix} $$

#!/usr/bin/python
# -*- coding: utf-8 -*-
import numpy as np
import cv2 as cv
import glob

def Main():
    criteria = (cv.TERM_CRITERIA_EPS + cv.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)
    objp *= 25

    # ホモグラフィの計算のために、ワールド座標系における適当な点を 4 つ用意します。
    objpCorners = np.float32([[0,0,0], [6,0,0], [0,5,0], [6,5,0]])
    objpCorners *= 25

    # 挿入したい画像を、位置を推定する物体 (キャリブレーションボード) の
    # ワールド座標系におけるサイズに応じて resize します。
    billboardImage = cv.resize(cv.imread('aaa.png'), dsize=(6*25, 6*25))

    # 白色の画像を同じサイズで用意します。
    maskImage = np.ones((6*25, 6*25), dtype=np.float32) * 255

    # キャリブレーションで推定した単一カメラの内部パラメータ
    with np.load('calib.npz') as data:
        KK, distCoeffs = [data[i] for i in ('KK', 'distCoeffs')]

    # 画像に写っている物体の位置姿勢を推定
    for fname in glob.glob('left*.jpg'):

        img = cv.imread(fname)
        gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)

        # キャリブレーションボードの位置姿勢を推定してみます。
        ret, corners = cv.findChessboardCorners(gray, (7,6), None)

        if ret == True:
            # 座標の精度を上げる
            corners2 = cv.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)

            # solvePnP を用いて外部パラメータ (物体の位置姿勢に相当) だけを推定します。
            err, rvecs, tvecs = cv.solvePnP(objp, corners2, KK, distCoeffs)

            # ホモグラフィの計算のために用意した、ワールド座標系における適当な 4 つの点を投影します。
            imgpts, _ = cv.projectPoints(objpCorners, rvecs, tvecs, KK, distCoeffs)

            # ホモグラフィ行列を計算します。
            ptsSrc = objpCorners[:,:2].astype(np.float32)
            ptsDst = imgpts.reshape(4, 2).astype(np.float32)
            h = cv.getPerspectiveTransform(ptsSrc, ptsDst)

            # 透視変換によって挿入したい位置姿勢に変換します。
            billboardImageWarped = cv.warpPerspective(billboardImage, h, dsize=img.shape[:2][::-1])
            maskImageWarped = cv.warpPerspective(maskImage, h, dsize=img.shape[:2][::-1])

            # 画像として扱うときは np.uint8
            billboardImageWarped = billboardImageWarped.astype(np.uint8)
            maskImageWarped = maskImageWarped.astype(np.uint8)

            # 色の反転
            maskImageWarpedInverted = 255 - maskImageWarped

            # 3 チャンネルに変換
            maskImageWarpedInverted = cv.cvtColor(maskImageWarpedInverted, cv.COLOR_GRAY2RGB)

            # マスク用画像で、挿入したい領域を 0 にします。
            imgMasked = cv.bitwise_and(img, maskImageWarpedInverted)

            # 画像を挿入します。
            dst = cv.bitwise_or(imgMasked, billboardImageWarped)

            cv.imshow('billboardImageWarped', billboardImageWarped)
            cv.imshow('maskImageWarped', maskImageWarped)
            cv.imshow('maskImageWarpedInverted', maskImageWarpedInverted)
            cv.imshow('imgMasked', imgMasked)
            cv.imshow('dst', dst)
            cv.waitKey(0)
    cv.destroyAllWindows()

if __name__ == '__main__':
    Main()

Uploaded Image

Uploaded Image

Related pages
    概要 特徴点が分かっている既知の物体については、単一カメラを用いて位置姿勢が推定できます。物体や全体のシーンが未知である場合は、例えばステレオ形式のカメラ二つを用いることで同様の結果を得ます。具体的には、距離情報を濃淡として保存した距離画像 (Depth Map; 奥行きマップ) を作成します。 エピポーラ幾何について