ワールド座標に固定された単一カメラが存在するとします。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 を用います。

#!/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()

画像の挿入

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

#!/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()