複数カメラを用いた奥行きマップの作成 (OpenCV)
[履歴] [最終更新] (2020/03/15 22:04:23)
最近の投稿
注目の記事

概要

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

Uploaded Image

エピポーラ幾何について

三次元空間の点を二次元画像に投影する際に失われる情報として「カメラから点までの距離」があります。この深さ情報は、複数のカメラから同じ点が含まれる画像を取得することで計算できます。

Uploaded Image

Epipolar Geometry

ピンホールモデルのカメラが左右に二つある状況を考えます。左側のカメラの画像平面だけであれば点 $x$ の実際の深さが分かりませんが、右側のカメラの画像平面内に対応する一つの点 $x'$ を見つけることができれば、二つのカメラの物理的な位置関係をもとに幾何的に深さ情報が計算できます。左側のカメラと右側のカメラそれぞれについてキャリブレーションを行うことが可能であれば、この物理的な位置関係は後述のとおりキャリブレーション結果の外部パラメータから計算できます。

対応する点 $x'$ は特徴量をマッチングすることで見つけます。上図から明らかなように、対応する点 $x'$ を探索する範囲は右側の画像平面すべてではなく、線分 $l'$ だけでよいことが分かります。これをエピポーラ拘束とよびます。このような線分 $l'$ を見つけることができれば信頼性と計算効率のどちらも高まります。

  • 線分 $l'$ を点 $x$ の エピポーラ線 (epiline) とよびます。
  • 平面 XOO' をエピポーラ平面 (Epipolar Plane) とよびます。
  • 二つのカメラ中心 $O$ と $O'$ による直線と画像平面の交点 $e$、$e'$ をエピポール (epipole) とよびます。

任意の点 $x$ に対応するエピポーラ線 $l'$ はエピポール $e'$ を通るため、エピポール $e'$ は複数のエピポーラ線 $l'$ の交点として計算できます。エピポールは画像平面内にあるとは限りません。

最終的な目標である、上図の左側の画像における、点 $x$ の深さ情報を得るためには、右側の画像において点 $x$ に対応するエピポーラ線 $l'$ を見つけて、特徴量マッチングを線分 $l'$ 上で行えばよいことが分かりました。また、歪みがある場合は事前にキャリブレーションで歪み補正を行っておく必要があります。

二つのカメラ間の回転行列と平行移動ベクトルの計算 (ステレオキャリブレーション)

左側のカメラと右側のカメラそれぞれについてキャリブレーションを行うことが可能である状況を考えると、任意の座標系、例えばワールド座標系からそれぞれのカメラ座標系への変換行列 $T_{WorldToCamera1}$ と $T_{WorldToCamera2}$ が外部パラメータとして得られます。これを利用すると、左のカメラから右のカメラへの変換行列が計算できます。変換行列には逆行列が存在することが保証されています。

$$\begin{eqnarray} T_{Camera1ToCamera2} &=& T_{WorldToCamera2} T_{Camera1ToWorld} \\ &=& T_{WorldToCamera2} T_{WorldToCamera1}^{-1} \end{eqnarray} $$

この変換行列に相当する情報を持つ行列として、ステレオキャリブレーションでは後述の基本行列 $E$ Essential Matrix を扱います。以降、$T_{WorldToCamera1}$ の回転行列を $R_l$、平行移動ベクトルを $t_l$ とします。$T_{WorldToCamera2}$ の回転行列を $R_r$、平行移動ベクトルを $t_r$ とします。

Uploaded Image

Epipolar Geometry

ワールド座標系において、左のカメラと右のカメラの投影中心 $O_l$ と $O_r$ の姿勢 (位置と向き) を考えます。ワールド座標系において、位置 $O_l$ から位置 $O_r$ へのベクトル $T$ と、$O_l$ の向きを $O_r$ の向きに変換する回転行列 $R$ は外部パラメータ $R_l$、$t_l$、$R_r$、$t_r$ から計算することができます。注意: 上記の図におけるベクトル $O_l O_r$ が $T$ です。

前述のエピポーラ幾何の図における点 $X$ の左のカメラ座標系における座標を $P_l$、右のカメラ座標系における座標を $P_r$、ワールド座標系における座標を $P$ とおくと、以下の式が得られます。

$$\begin{eqnarray} P_l &=& R_l P + t_l \\ P_r &=& R_r P + t_r \end{eqnarray} $$

また、二つのカメラ座標系の間には、幾何的に以下の関係が成立します。

$$P_r = R^T (P_l - T) $$

回転行列の転置行列は逆行列と一致することも考慮して変形すると以下のようになります。

$$R P_r = P_l - T $$

よって

$$\begin{eqnarray} T &=& P_l - R P_r \\ &=& t_l - R t_r + (R_l - R R_r) P \end{eqnarray} $$

これは座標 $P$ によらず成立するため以下の式が成り立ちます。

$$R_l - R R_r = 0 $$

以上から、ワールド座標系において、左のカメラと右のカメラの投影中心について、位置 $O_l$ から位置 $O_r$ へのベクトル $T$ と $O_l$ の向きを $O_r$ の向きに変換する回転行列 $R$ は、外部パラメータ $R_l$、$t_l$、$R_r$、$t_r$ から以下のように計算することができることが分かります。

$$\begin{eqnarray} T &=& t_l - R t_r \\ R &=& R_l R_r^T \end{eqnarray} $$

基本行列 Essential Matrix とエピポーラ線の計算

上記の回転行列 $R$ と平行移動ベクトル $T$ を用いると、左の画像平面上の点 $x$ から右の画像平面のエピポーラ線 $l'$ を計算するための行列として利用できる、基本行列 $E$ Essential Matrix が計算できます。注意: 以下の図におけるベクトル $O_l O_r$ が $T$ です。

Uploaded Image

Uploaded Image

Epipolar Geometry

点 $X$ の左のカメラ座標系における座標 $P_l$ と右のカメラ座標系における座標 $P_r$ について、前述のとおり、二つの座標系の幾何的な関係から以下の式が成り立ちます。

$$P_r = R^T (P_l - T) $$

また $P_l$ はエピポーラ平面上の点であることを考えると、点 $X$ の位置に関わらず常に以下の式が成り立ちます。一般的な平面の方程式であり、平面上のベクトルと平面の法線ベクトルの内積が 0 であることを表現しています。

$$(P_l - T)^T (P_l \times T) = 0 $$

上記二つの式について、最初の式を二つ目の式に代入すると以下のようになります。

$$\begin{eqnarray} (R P_r)^T (P_l \times T) &=& 0 \\ P_r^T R^T (P_l \times T) &=& 0 \end{eqnarray} $$

平行移動ベクトル $T$ の要素をもとに、以下のような行列 $S$ を定義すると、

$$S = \begin{pmatrix} 0 & -T_z & T_y \\ T_z & 0 & -T_x \\ -T_y & T_x & 0 \end{pmatrix} $$

以下の式が成り立ちます。

$$T \times P_l = S P_l $$

ここまでの結果から以下の式が得られます。

$$P_r^T R^T S P_l = 0 $$

基本行列 $E$ を以下のように定義します。

$$E = R^T S $$

これにより以下の式が得られます。

$$P_r^T E P_l = 0 $$

更に直線 $OX$ と左の画像平面の交点 $x$ についてベクトル $Ox$ を $p_l$ とします。同様に右のカメラについては $p_r$ とします。左のカメラと右のカメラの焦点距離を $f_l$、 $f_r$ とし、投影中心 $O$ と $O'$ から点 $X$ までの距離を $z_l$、$z_r$ とすると、以下の式が成り立ちます。

$$\begin{eqnarray} p_l &=& \frac{f_l}{z_l} P_l \\ p_r &=& \frac{f_r}{z_r} P_r \end{eqnarray} $$

これを先程の式に代入すると

$$\begin{eqnarray} ( \frac{z_r}{f_r} p_r )^T E \frac{z_l}{f_l} p_l &=& 0 \\ \frac{z_r}{f_r}\frac{z_l}{f_l} p_r^T E p_l &=& 0 \\ p_r^T E p_l &=& 0 \\ \end{eqnarray} $$

ここで行列 $S$ の階数を調べてみると、ランク落ちしていることが分かります。

行列 $S$ は正方行列であり、その固有値 $\lambda$ は以下の行列式で計算できます。

$$\begin{eqnarray} \mathrm{det} (\lambda I - S) &=& |\lambda I - S| = \left| \begin{array}{ccc} \lambda & T_z & -T_y \\ -T_z & \lambda & T_x \\ T_y & -T_x & \lambda \end{array} \right| \\ &=& \lambda^3 + \lambda (T_x^2 + T_y^2 + T_z^2) \end{eqnarray} $$

よって行列 $S$ の固有値は以下の三つです。

$$\begin{eqnarray} \lambda &=& 0 \\ \lambda &=& \sqrt{T_x^2 + T_y^2 + T_z^2} \, \mathrm{i} \\ \lambda &=& -\sqrt{T_x^2 + T_y^2 + T_z^2} \, \mathrm{i} \end{eqnarray} $$

0 でない固有値の個数は 2 であり、行列 $S$ の階数は 2 です。したがって以下の連立方程式で点 $p_l$ が指定された場合に $p_r$ は一意には定まらず、直線の方程式が得られます。これがエピポーラ線です。行列 $S$ の定義から明らかなように、基本行列は二つのカメラのキャリブレーション結果の外部パラメータから計算できます。

$$p_r^T E p_l = 0 $$

基礎行列 Fundamental Matrix とエピポーラ線の計算

しかしながら実際に得られる情報は空間の座標 $p_l$ ではなくカメラで取得された画像における点 $x$ のピクセル座標 $q_l$ であるため、カメラ座標やワールド座標ではなく、ピクセル座標で二つのカメラの情報を関連付ける行列が必要になります。そのような行列は、基本行列にそれぞれのカメラのキャリブレーションで得られる内部パラメータの情報を加えた行列になります。この行列は基礎行列 $F$ Fundamental Matrix とよばれます。基礎行列を利用すると、片方の画像における点から、もう一つの画像におけるエピポーラ線を計算できます。

Uploaded Image

Uploaded Image

Epipolar Geometry

点 $X$ について、左のカメラ座標系における座標は $P_l$ です。左のカメラの内部パラメータ $K_l$ を用いると、左の画像平面内におけるピクセル座標は以下のようになります。

$$q_l = K_l P_l $$

同様に右のカメラについて以下が得られます。

$$q_r = K_r P_r $$

基本行列の計算で導いた式のうち以下のものを考えます。

$$P_r^T E P_l = 0 $$

先程の二つを代入します。

$$\begin{eqnarray} (K_r^{-1} q_r)^T E K_l^{-1} q_l &=& 0 \\ q_r^T (K_r^{-1})^T E K_l^{-1} q_l &=& 0 \end{eqnarray} $$

基礎行列を以下のように定義します。

$$F = (K_r^{-1})^T E K_l^{-1} $$

代入して以下の式が得られます。

$$q_r^T F q_l = 0 $$

基礎行列の階数は基本行列と同様に 2 です。これは左の画像平面におけるピクセル座標 $q_l$ が定まると、右の画像平面におけるエピポーラ線がピクセル座標で得られることを意味します。

キャリブレーションを用いない基礎行列とエピポーラ線の計算

基礎行列は上述のようにそれぞれのカメラのキャリブレーション結果から計算することができますが、その他の方法として、二つの画像における対応点から計算することもできます。ホモグラフィの計算で対応点を利用するのと同様です。対応点の組は理論上少なくとも 7 個必要です。以下の例では、特徴量のマッチングを行うことで、後者の方法で基礎行列を計算しています。特徴点の検出に SIFT を利用する場合は古い OpenCV が必要になります。

python -m pip install opencv-python==3.4.1.15
python -m pip install opencv-contrib-python==3.4.1.15

Uploaded Image

left.jpg, right.jpg

エピポール線の交点がエピポーラです。左右の画像の両方について、もう片方のカメラは画像内には存在せず、したがってエピポーラは画像の外に存在することが分かります。

#!/usr/bin/python
# -*- coding: utf-8 -*-

import numpy as np
import cv2 as cv

# カラー画像である必要はないため、グレースケールで画像を読み込みます。
img1 = cv.imread('left.jpg', cv.IMREAD_GRAYSCALE)
img2 = cv.imread('right.jpg', cv.IMREAD_GRAYSCALE)

# 特徴点の検出に SIFT アルゴリズムを利用します。
sift = cv.xfeatures2d.SIFT_create()

# 特徴点 Key Points kp1, kp2
# 特徴量記述子 Feature Description des1, des2
kp1, des1 = sift.detectAndCompute(img1, None)
kp2, des2 = sift.detectAndCompute(img2, None)

# 特徴量を FLANN を利用してマッチングします。
# マッチング度合いが高い順に二つ (k=2) 取得します。
FLANN_INDEX_KDTREE = 1
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks=50)
flann = cv.FlannBasedMatcher(index_params, search_params)
matches = flann.knnMatch(des1, des2, k=2)

# マッチング結果に閾値を設定します。
# 取得した結果二つのうち、一つをもう一つの閾値として利用しています。
good = []
pts1 = []
pts2 = []
for i,(m, n) in enumerate(matches):
    if m.distance < 0.8 * n.distance:
        good.append(m)
        pts1.append(kp1[m.queryIdx].pt)
        pts2.append(kp2[m.trainIdx].pt)
pts1 = np.int32(pts1)
pts2 = np.int32(pts2)

# 基礎行列と、基礎行列の計算に実際に利用された点の組を返します。
F, mask = cv.findFundamentalMat(pts1,pts2,cv.FM_LMEDS)

# 実際に使用された対応点のみを描画のために取得します。
pts1 = pts1[mask.ravel() == 1]
pts2 = pts2[mask.ravel() == 1]

# 左の画像 (第二引数に1を指定) における特徴点に対応するエピポーラ線を計算してみます。
epilines = cv.computeCorrespondEpilines(pts1, 1, F)
epilines = epilines.reshape(-1, 3)

# 色付きの点や線を描画するために BGR に変換します。
dst_left = cv.cvtColor(img1, cv.COLOR_GRAY2BGR)
dst_right = cv.cvtColor(img2, cv.COLOR_GRAY2BGR)

# 右側の画像の横幅ピクセル数
_, width = img2.shape

# 左側の画像の特徴点、右側の画像の対応する特徴点、右側の画像の対応するエピポーラ線
for pt1, pt2, epiline in zip(pts1, pts2, epilines):
    # 分かりやすさのため乱数で色を決定します。
    color = tuple(np.random.randint(0, 255, 3).tolist())

    # 左側の画像の特徴点
    dst_left = cv.circle(dst_left, tuple(pt1), 5, color, -1)

    # 右側の画像の対応する特徴点
    dst_right = cv.circle(dst_right, tuple(pt2), 5, color, -1)

    # 右側の画像の対応するエピポーラ線 (a x + b y + c = 0)
    # ピクセル座標は int です。描画するために二点指定する必要があります。
    # 最初の点は x=0 として y を計算しています。
    x0, y0 = map(int, [0, -epiline[2] / epiline[1]])
    # 二つ目の点は x=width として y を計算しています。
    x1, y1 = map(int, [width, -(epiline[2] + epiline[0] * width) / epiline[1]])
    dst_right = cv.line(dst_right, (x0, y0), (x1, y1), color, 1)

cv.imshow('left camera', dst_left)
cv.imshow('right camera', dst_right)
cv.waitKey(0)

ステレオキャリブレーションによる基礎行列の計算

前述のとおり、カメラキャリブレーションで基礎行列を計算できます。この場合、基礎行列の計算のために特徴量マッチングを行う必要はありません。

Uploaded Image

stereoData

#!/usr/bin/python
# -*- coding: utf-8 -*-

import cv2 as cv
import numpy as np
from os import path

def Main():
    # チェスボードの白黒の交点 (コーナー) の個数
    boardNx = 9
    boardNy = 6

    # ピンホールモデルで表現できるカメラで取得した画像を読み込みます。
    imagesLeft, imagesRight, filenamesLeft, filenamesRight = ReadImages()

    # 一般性を失わず、チェスボードのコーナーの位置が x座標と y座標と一致するようなワールド座標を仮定できます。
    objectPoints = GetChessboardObjectPoints(boardNx, boardNy)

    # チェスボードのコーナーを検出します。
    cornersLeftList, cornersRightList, objectPointsList = FindChessboardCorners(imagesLeft, imagesRight, filenamesLeft, filenamesRight, boardNx, boardNy, objectPoints)

    # ステレオキャリブレーションを行います。
    imageSize = imagesLeft[0].shape[::-1]
    KKLeft, distCoeffsLeft, KKRight, distCoeffsRight, R, T, E, F = RunStereoCalibration(objectPointsList, cornersLeftList, cornersRightList, imageSize)

    # ステレオキャリブレーション結果を qr^T*F*ql = 0 で検証します。
    # エピポーラ線を基礎行列 F から計算することでこれに相当する検証を行えます。
    # 基礎行列 F は R, T, E, KK の情報を含むため、F の検証でそれらの検証が行えます。
    avgErr = CheckCalibrationQuality(boardNx, boardNy, cornersLeftList, cornersRightList, KKLeft, distCoeffsLeft, KKRight, distCoeffsRight, F)

def ReadImages(stereoDataDir = './stereoData'):
    imagesLeft = []
    imagesRight = []
    filenamesLeft = []
    filenamesRight = []
    for i in range(1, 15):
        filenameLeft = 'left%02d.jpg' % i
        filenameRight = 'right%02d.jpg' % i
        imagesLeft.append(cv.imread(path.join(stereoDataDir, filenameLeft), cv.IMREAD_GRAYSCALE))
        imagesRight.append(cv.imread(path.join(stereoDataDir, filenameRight), cv.IMREAD_GRAYSCALE))
        filenamesLeft.append(filenameLeft)
        filenamesRight.append(filenameRight)
    return imagesLeft, imagesRight, filenamesLeft, filenamesRight

def GetChessboardObjectPoints(boardNx, boardNy):
    objectPoints = []
    for ny in range(boardNy):
        for nx in range(boardNx):
            objectPoints.append([ny, nx, 0])
    return np.float32(objectPoints)

def FindChessboardCorners(imagesLeft, imagesRight, filenamesLeft, filenamesRight, boardNx, boardNy, objectPoints):
    cornersLeftList = []
    cornersRightList = []
    objectPointsList = []
    criteria = (cv.TERM_CRITERIA_EPS | cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
    for imageLeft, imageRight, filenameLeft, filenameRight in zip(imagesLeft, imagesRight, filenamesLeft, filenamesRight):
        foundLeft, cornersLeft = cv.findChessboardCorners(imageLeft, (boardNx, boardNy), None)
        foundRight, cornersRight = cv.findChessboardCorners(imageRight, (boardNx, boardNy), None)
        bothExist = foundLeft and foundRight
        bothNone = (not foundLeft) and (not foundRight)
        assert(bothExist or bothNone)
        if not bothExist:
            continue
        corners2Left = cv.cornerSubPix(imageLeft, cornersLeft, (11, 11), (-1, -1), criteria)
        corners2Right = cv.cornerSubPix(imageRight, cornersRight, (11, 11), (-1, -1), criteria)
        cornersLeftList.append(corners2Left)
        cornersRightList.append(corners2Right)
        objectPointsList.append(objectPoints)
        cv.drawChessboardCorners(imageLeft, (boardNx, boardNy), corners2Left, foundLeft)
        cv.drawChessboardCorners(imageRight, (boardNx, boardNy), corners2Right, foundRight)
        cv.imshow(filenameLeft, imageLeft)
        cv.imshow(filenameRight, imageRight)
        cv.waitKey(0)
        cv.destroyAllWindows()
    return cornersLeftList, cornersRightList, objectPointsList

def RunStereoCalibration(objectPoints, cornersLeft, cornersRight, imageSize):
    criteria = (cv.TERM_CRITERIA_COUNT | cv.TERM_CRITERIA_EPS, 100, 1e-5)
    # 注意: 既定では flags が cv.CALIB_FIX_INTRINSIC となっており、内部パラメータ (KK と distCoeffs) の推定が行われません。
    res = cv.stereoCalibrate(objectPoints, cornersLeft, cornersRight, None, None, None, None, imageSize, criteria=criteria, flags=0)
    err, KKLeft, distCoeffsLeft, KKRight, distCoeffsRight, R, T, E, F = res
    print('err = %r' % err)
    print('KKLeft = %r' % KKLeft)
    print('distCoeffsLeft = %r' % distCoeffsLeft)
    print('KKRight = %r' % KKRight)
    print('distCoeffsRight = %r' % distCoeffsRight)
    print('R = %r' % R)
    print('T = %r' % T)
    print('E = %r' % E)
    print('F = %r' % F)
    return KKLeft, distCoeffsLeft, KKRight, distCoeffsRight, R, T, E, F

def CheckCalibrationQuality(boardNx, boardNy, cornersLeftList, cornersRightList, KKLeft, distCoeffsLeft, KKRight, distCoeffsRight, F):
    N = boardNx * boardNy
    avgErr = 0.0
    for cornersLeft, cornersRight in zip(cornersLeftList, cornersRightList):
        cornersLeft = cv.undistortPoints(cornersLeft, KKLeft, distCoeffsLeft, None, KKLeft)
        cornersRight = cv.undistortPoints(cornersRight, KKRight, distCoeffsRight, None, KKRight)
        epilinesForPointsLeft = cv.computeCorrespondEpilines(cornersLeft, 1, F)
        epilinesForPointsRight = cv.computeCorrespondEpilines(cornersRight, 2, F)
        for i in range(N):
            xl = cornersLeft[i][0][0]
            yl = cornersLeft[i][0][1]
            al = epilinesForPointsLeft[i][0][0]
            bl = epilinesForPointsLeft[i][0][1]
            cl = epilinesForPointsLeft[i][0][2]
            xr = cornersRight[i][0][0]
            yr = cornersRight[i][0][1]
            ar = epilinesForPointsRight[i][0][0]
            br = epilinesForPointsRight[i][0][1]
            cr = epilinesForPointsRight[i][0][2]
            err = abs(ar*xl + br*yl + cr) + abs(al*xr + bl*yr + cl)
            avgErr += err
    avgErr = avgErr / (len(cornersLeft) * N)
    print('avgErr = %r' % avgErr)
    return avgErr

if __name__ == '__main__':
    Main()

実行例

err = 0.4370546667948499
KKLeft = array([[535.77916141,   0.        , 341.77622024],
       [  0.        , 535.6453919 , 235.15726332],
       [  0.        ,   0.        ,   1.        ]])
distCoeffsLeft = array([[-0.27071132,  0.01153839,  0.00165472, -0.00042276,  0.11938723]])
KKRight = array([[539.39949437,   0.        , 327.44298924],
       [  0.        , 539.06024174, 249.06964702],
       [  0.        ,   0.        ,   1.        ]])
distCoeffsRight = array([[-0.28139545,  0.10398873, -0.00045729,  0.00088085, -0.01748507]])
R = array([[ 0.99998698,  0.00379143,  0.00341591],
       [-0.00377495,  0.99998127, -0.00481747],
       [-0.00343411,  0.00480451,  0.99998256]])
T = array([[-3.33734382e+00],
       [ 3.87005742e-02],
       [ 2.47888063e-03]])
E = array([[-1.23544340e-04, -2.29289681e-03,  3.87118412e-02],
       [-8.98195415e-03,  1.60437093e-02,  3.33729409e+00],
       [-2.61017634e-02, -3.33742805e+00,  1.59453572e-02]])
F = array([[ 6.31011733e-09,  1.17140625e-07, -1.08906472e-03],
       [ 4.59048581e-07, -8.20164645e-07, -9.13475928e-02],
       [ 6.02708130e-04,  9.21357703e-02,  1.00000000e+00]])
avgErr = 0.06654294331868489

ステレオキャリブレーション結果を再利用できるように、何らかの形式で保存しておきます。

np.savez_compressed('stereoCalib', KKLeft=KKLeft, distCoeffsLeft=distCoeffsLeft, KKRight=KKRight, distCoeffsRight=distCoeffsRight, R=R, T=T, E=E, F=F)

生成されたファイル

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

ステレオ平行化 Stereo Rectification

ステレオキャリブレーションによって内部パラメータおよび互いの位置関係が明らかになっている二つのカメラを用いて取得した画像から、対応する点を見つけることができれば、その点の二つのカメラに対する相対的な位置を計算できることは前述のとおりです。片方の画像のある点に対応する点は、対応するエピポーラ線上を探索すれば十分であることも前述のとおりです。

ここではこれらの処理を簡単にするために、エピポーラ線をもう片方の画像の同じ高さにある行と一致するようにします。これはステレオ平行化とよばれます。OpenCV で実装されているアルゴリズムの詳細とは異なりますが、おおよそ以下のような流れで平行化します。

左のカメラの内部パラメータ $K_l$、外部パラメータの回転行列 $R_l$、外部パラメータの平行移動ベクトル $t_l$ はカメラキャリブレーションによって既知です。同様に右のカメラについても $K_r$、$R_r$、$t_r$ は既知です。ここでワールド座標系におけるある点 $X$ の座標 $P$ は、左のカメラ座標においては $P_l$、右のカメラ座標においては $P_r$ であるとします。この点について以下のような関係式が成り立ちます。

$$\begin{eqnarray} P_l &=& R_l P + t_l \\ P_r &=& R_r P + t_r \end{eqnarray} $$

また、実際の画像におけるピクセル座標を左のカメラと右のカメラについてそれぞれ $q_l$、$q_r$ とすると以下の式が成り立ちます。

$$\begin{eqnarray} q_l &=& K_l P_l \\ q_r &=& K_r P_r \end{eqnarray} $$

二つのカメラ座標系の幾何的な関係から以下も成り立ちます。ここで $R$ と $T$ は二つのカメラの位置関係を表現する、ステレオキャリブレーションの結果として得られた既知のパラメータです。

$$P_r = R^T (P_l - T) $$

$q_r = K_r P_r$ に左から $K_r^{-1}$ をかけて $P_r = R^T (P_l - T)$ と $P_l = R_l P + t_l$ を代入すると以下のようになります。

$$\begin{eqnarray} K_r^{-1} q_r &=& P_r \\ &=& R^T (P_l - T) \\ R K_r^{-1} q_r &=& P_l - T \\ &=& R_l P + t_l - T \end{eqnarray} $$

平行移動ベクトル $T$ を左のカメラ座標系における任意の方向に回転させる行列を考えることは常に可能です。ここでは特に画像平面と横方向に平行になるように回転させる行列 $L$ を考えて、回転後のベクトルを $b$ とおきます。

$$L T = b $$

先程の $R K_r^{-1} q_r = R_l P + t_l - T$ に左から $K_l L$ をかけると以下のようになります。

$$K_l L R K_r^{-1} q_r = K_l (L R_l P + L t_l - b) $$

同様に $q_l = K_l P_l = K_l (R_l P + t_l)$ に左から $K_l L K_l^{-1}$ をかけると以下のようになります。

$$K_l L K_l^{-1} q_l = K_l (L R_l P + L t_l) $$

以下のように変数を定義します。

$$\begin{eqnarray} q'_l &=& K_l L K_l^{-1} q_l \\ q'_r &=& K_l L R K_r^{-1} q_r \\ P'_l &=& L R_l P + L t_l \end{eqnarray} $$

最終的に以下の式が得られます。

$$\begin{eqnarray} q'_l &=& K_l P'_l \\ q'_r &=& K_l (P'_l - b) \end{eqnarray} $$

新しいピクセル座標 $q'_l$ と $q'_r$ は座標 $P'_l$ と $P'_l - b$ が投影されたものであり、これらはともに左のカメラ座標系の点です。$b$ が左のカメラ座標系の画像平面の横方向と平行になるように $L$ を定義したので、新しいピクセル座標による画像について、片方の画像の点のエピポーラ線は、もう片方の画像の同じ高さにある行と一致します。

以下はステレオキャリブレーションのサンプルコードです。

ステレオ平行化前の画像

Uploaded Image

ステレオ平行化後の画像

Uploaded Image

#!/usr/bin/python
# -*- coding: utf-8 -*-

import cv2 as cv
import numpy as np
from os import path

def Main():
    # ステレオキャリブレーション結果を読み込みます。
    KKLeft, distCoeffsLeft, KKRight, distCoeffsRight, R, T, E, F = LoadParams()

    # ステレオキャリブレーション済みのカメラで取得した画像を読み込みます。
    # ここではステレオキャリブレーションに用いた画像を用いていますが、本来その必要はありません。
    imagesLeft, imagesRight, filenamesLeft, filenamesRight = ReadImages()
    imageSize = imagesLeft[0].shape[::-1]

    # ステレオ平行化を行います。
    imagesRectifiedLeft, imagesRectifiedRight, Q, validPixROIl, validPixROIr = StereoRectify(imagesLeft, imagesRight, imageSize, KKLeft, distCoeffsLeft, KKRight, distCoeffsRight, R, T)

    # 描画してみます。
    CheckRectifiedImages(filenamesLeft, filenamesRight, imageSize, imagesLeft, imagesRight, imagesRectifiedLeft, imagesRectifiedRight)

    # 次のステップのためにステレオ平行化を行った画像を保存しておきます。
    SaveRectifiedImages(filenamesLeft, filenamesRight, imagesRectifiedLeft, imagesRectifiedRight, Q, validPixROIl, validPixROIr)

def LoadParams(filepath='stereoCalib.npz'):
    with np.load(filepath) as data:
        KKLeft, distCoeffsLeft, KKRight, distCoeffsRight, R, T, E, F = [data[i] for i in ('KKLeft', 'distCoeffsLeft', 'KKRight', 'distCoeffsRight', 'R', 'T', 'E', 'F')]
    return KKLeft, distCoeffsLeft, KKRight, distCoeffsRight, R, T, E, F

def ReadImages(stereoDataDir = './stereoData'):
    imagesLeft = []
    imagesRight = []
    filenamesLeft = []
    filenamesRight = []
    for i in range(1, 15):
        filenameLeft = 'left%02d.jpg' % i
        filenameRight = 'right%02d.jpg' % i
        imagesLeft.append(cv.imread(path.join(stereoDataDir, filenameLeft), cv.IMREAD_GRAYSCALE))
        imagesRight.append(cv.imread(path.join(stereoDataDir, filenameRight), cv.IMREAD_GRAYSCALE))
        filenamesLeft.append(filenameLeft)
        filenamesRight.append(filenameRight)
    return imagesLeft, imagesRight, filenamesLeft, filenamesRight

def StereoRectify(imagesLeft, imagesRight, imageSize, KKLeft, distCoeffsLeft, KKRight, distCoeffsRight, R, T):
    Rl, Rr, Pl, Pr, Q, validPixROIl, validPixROIr = cv.stereoRectify(KKLeft, distCoeffsLeft, KKRight, distCoeffsRight, imageSize, R, T, alpha=1, flags=0)
    mapXLeft, mapYLeft = cv.initUndistortRectifyMap(KKLeft, distCoeffsLeft, Rl, Pl, imageSize, cv.CV_32FC1)
    mapXRight, mapYRight = cv.initUndistortRectifyMap(KKRight, distCoeffsRight, Rr, Pr, imageSize, cv.CV_32FC1)
    imagesRectifiedLeft = []
    imagesRectifiedRight = []
    for imageLeft in imagesLeft:
        imageRectifiedLeft = cv.remap(imageLeft, mapXLeft, mapYLeft, cv.INTER_LINEAR)
        imagesRectifiedLeft.append(imageRectifiedLeft)
    for imageRight in imagesRight:
        imageRectifiedRight = cv.remap(imageRight, mapXRight, mapYRight, cv.INTER_LINEAR)
        imagesRectifiedRight.append(imageRectifiedRight)
    return imagesRectifiedLeft, imagesRectifiedRight, Q, validPixROIl, validPixROIr

def CheckRectifiedImages(filenamesLeft, filenamesRight, imageSize, imagesLeft, imagesRight, imagesRectifiedLeft, imagesRectifiedRight):
    for filenameLeft, filenameRight, imageLeft, imageRight, imageRectifiedLeft, imageRectifiedRight in zip(
            filenamesLeft, filenamesRight, imagesLeft, imagesRight, imagesRectifiedLeft, imagesRectifiedRight):
        h = imageSize[1]
        w = imageSize[0]
        imageLR = np.zeros((h, 2 * w, 3), dtype=np.uint8)
        imageRectifiedLR = np.zeros((h, 2 * w, 3), dtype=np.uint8)
        imageLR[:, :w, :] = cv.cvtColor(imageLeft, cv.COLOR_GRAY2RGB)
        imageLR[:, w:, :] = cv.cvtColor(imageRight, cv.COLOR_GRAY2RGB)
        imageRectifiedLR[:, :w, :] = cv.cvtColor(imageRectifiedLeft, cv.COLOR_GRAY2RGB)
        imageRectifiedLR[:, w:, :] = cv.cvtColor(imageRectifiedRight, cv.COLOR_GRAY2RGB)
        for i in range(5, h, 20):
            cv.line(imageLR, (0, i), (2 * w, i), (0, 255, 0));
            cv.line(imageRectifiedLR, (0, i), (2 * w, i), (0, 255, 0));
        cv.imshow(filenameLeft + '-' + filenameRight, imageLR)
        cv.imshow(filenameLeft + '-' + filenameRight + '-rectified', imageRectifiedLR)
        cv.waitKey(0)
        cv.destroyAllWindows()

def SaveRectifiedImages(filenamesLeft, filenamesRight, imagesRectifiedLeft, imagesRectifiedRight, Q, validPixROIl, validPixROIr):
    np.savez_compressed('stereoRect', Q=Q, validPixROIl=validPixROIl, validPixROIr=validPixROIr)
    for filenameLeft, filenameRight, imageRectifiedLeft, imageRectifiedRight in zip(
            filenamesLeft, filenamesRight, imagesRectifiedLeft, imagesRectifiedRight):
        cv.imwrite('rect-' + filenameLeft, imageRectifiedLeft)
        cv.imwrite('rect-' + filenameRight, imageRectifiedRight)

if __name__ == '__main__':
    Main()

対応点探索、視差マップの作成、再投影による奥行きマップの作成

ステレオキャリブレーションおよびステレオ平行化が完了している、以下の図のような状況を考えます。

Uploaded Image

Depth Map from Stereo Images

ステレオキャリブレーションの結果として、投影中心 $O$ から $O'$ までの距離 $B$ は既知です。またステレオ平行化も完了しているため、投影中心から画像平面までの焦点距離 $f$ は左右のカメラで同じ値となり、その値も既知です。点 $X$ の左の画像および右の画像におけるピクセル座標をそれぞれ $x \gt 0$、$x' \lt 0$ とおきます。三角形 $X x x'$ と $X O O'$ の相似を考えると、左右のカメラから点 $X$ までの距離 $z$ について以下の式が成り立ちます。

$$\begin{eqnarray} z : z - f &=& B : B - x + x' \\ B (z - f) &=& z (B - x + x') \\ -B f &=& -z(x - x') \\ z &=& \frac{B f}{x - x'} \end{eqnarray} $$

二つのカメラで取得した画像における各点の対応づけを行うことで、各ピクセルに関する $x - x'$ が格納された視差マップを作成します。二つのカメラの幾何的な配置を考慮して、上記三角測量の考え方を用いることで、視差マップを距離情報に変換します。

以下のサンプルでは対応点の探索のために SGBM (Semi-Global Block Matching) 法を利用しています。その他に OpenCV には BM (Block Matching) 法も実装されています。いずれの場合についても、カメラ近くの物体の視差が必要な場合は numDisparities に大きな値を指定する必要があることに注意します。近くの物体については視差が大きくなるため探索する範囲を広げる必要があります。

#!/usr/bin/python
# -*- coding: utf-8 -*-

import cv2 as cv
import numpy as np
from os import path
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt

def Main():
    # ステレオ平行化済みの画像を読み込みます。
    rectifiedImagesLeft, rectifiedImagesRight, filenamesLeft, filenamesRight = ReadRectifiedImages()

    # ステレオ平行化時に出力されたパラメータを読み込みます。
    Q, validPixROI = LoadStereoRectificationParameters()

    # 視差マップを作成します。
    # 注意: 片方のカメラ画像における点と対応する点を、もう片方のカメラ画像から探索するにあたり
    # numDisparities ピクセル分のずれの範囲内のみが探索対象となります。カメラに近い物体の視差は大きいため、
    # 近い物体の視差マップが必要な場合は値を大きくする必要があります。ただしその場合、遠くの物体の視差は得られなくなります。
    numDisparitiesList = [110, 220, 220, 220, 220, 110, 110, 220, 220, 220, 220, 220, 220, 220]
    disparityImages = ComputeDisparityImages(rectifiedImagesLeft, rectifiedImagesRight, numDisparitiesList)

    # 深さ情報を計算します。
    depthMaps = ComputeDepthMaps(disparityImages, Q)

    # 可視化してみます。
    CheckDepthMaps(filenamesLeft, rectifiedImagesLeft, validPixROI, disparityImages, depthMaps)

def ReadRectifiedImages(rectifiedDataDir = '.'):
    rectifiedImagesLeft = []
    rectifiedImagesRight = []
    filenamesLeft = []
    filenamesRight = []
    for i in range(1, 15):
        filenameLeft = 'rect-left%02d.jpg' % i
        filenameRight = 'rect-right%02d.jpg' % i
        rectifiedImagesLeft.append(cv.imread(path.join(rectifiedDataDir, filenameLeft), cv.IMREAD_GRAYSCALE))
        rectifiedImagesRight.append(cv.imread(path.join(rectifiedDataDir, filenameRight), cv.IMREAD_GRAYSCALE))
        filenamesLeft.append(filenameLeft)
        filenamesRight.append(filenameRight)
    return rectifiedImagesLeft, rectifiedImagesRight, filenamesLeft, filenamesRight

def LoadStereoRectificationParameters(filepath='./stereoRect.npz'):
    with np.load(filepath) as data:
        Q, validPixROIl, validPixROIr = [data[i] for i in ('Q', 'validPixROIl', 'validPixROIr')]
    a = max(validPixROIl[0], validPixROIr[0])
    b = max(validPixROIl[1], validPixROIr[1])
    c = min(validPixROIl[2], validPixROIr[2])
    d = min(validPixROIl[3], validPixROIr[3])
    validPixROI = np.array([a, b, c, d])
    return Q, validPixROI

def ComputeDisparityImages(rectifiedImagesLeft, rectifiedImagesRight, numDisparitiesList, windowSize=11):
    disparityImages = []
    for rectifiedImageLeft, rectifiedImageRight, numDisparities in zip(rectifiedImagesLeft, rectifiedImagesRight, numDisparitiesList):
        stereo = cv.StereoSGBM_create(
            numDisparities = numDisparities,
            blockSize = 16,
            P1 = 8 * 3 * windowSize**2,
            P2 = 32 * 3 * windowSize**2,
            disp12MaxDiff = 1,
            uniquenessRatio = 10,
            speckleWindowSize = 100,
            speckleRange = 32
        )
        # int では分解能が荒くなるため、ここでは float を用いています。
        disparity = stereo.compute(rectifiedImageLeft, rectifiedImageRight).astype(np.float32)
        cv.normalize(disparity, disparity, 0, 1.0, cv.NORM_MINMAX)
        disparityImages.append(disparity)
    return disparityImages

def ComputeDepthMaps(disparityImages, Q):
    return map(lambda disparityImage: cv.reprojectImageTo3D(disparityImage, Q), disparityImages)

def CheckDepthMaps(filenamesLeft, rectifiedImagesLeft, validPixROI, disparityImages, depthMaps):
    a, b, c, d = validPixROI
    for filenameLeft, rectifiedImageLeft, disparityImage, depthMap in zip(
            filenamesLeft, rectifiedImagesLeft, disparityImages, depthMaps):
        # ステレオキャリブレーション結果
        cv.imshow('rect-{}'.format(filenameLeft), rectifiedImageLeft[b:d, a:c])
        # 視差マップ
        cv.imshow('disparity-{}'.format(filenameLeft), disparityImage[b:d, a:c])
        # 奥行マップ (各ピクセルに対応する x, y, z 座標)
        xRangeNormalized = depthMap[b:d, a:c, 0].astype(np.int32)
        yRangeNormalized = depthMap[b:d, a:c, 1].astype(np.int32)
        zRangeNormalized = depthMap[b:d, a:c, 2].astype(np.int32)
        cv.normalize(xRangeNormalized, xRangeNormalized, 0, 255, cv.NORM_MINMAX)
        cv.normalize(yRangeNormalized, yRangeNormalized, 0, 255, cv.NORM_MINMAX)
        cv.normalize(zRangeNormalized, zRangeNormalized, 0, 255, cv.NORM_MINMAX)
        cv.imshow('xRange', xRangeNormalized.astype(np.uint8))
        cv.imshow('yRange', yRangeNormalized.astype(np.uint8))
        cv.imshow('zRange', zRangeNormalized.astype(np.uint8))
        cv.waitKey(0)
        # ポイントクラウドとして可視化
        fig = plt.figure()
        ax = fig.add_subplot(111, projection='3d')
        ax.set_xlabel('X')
        ax.set_ylabel('Y')
        ax.set_zlabel('Z')
        points = depthMap[b:d, a:c].reshape(-1, 3)
        zRange = depthMap[b:d, a:c, 2]
        zMin = zRange.min()
        zMax = zRange.max()
        for i, (x, y, z) in enumerate(points):
            # すべて描画すると処理が重いため一部のみ描画しています。
            if i % 16 != 0:
                continue
            # 見やすさのため、z の値によって色を変えています。
            color = 1.0 - (z - zMin) / (zMax - zMin)
            # 見やすさのために正負を調整
            ax.scatter3D(-x, y, -z, c=np.float32([0, color, 0]))
        plt.show()
        cv.destroyAllWindows()

if __name__ == '__main__':
    Main()

ポイントクラウドとして可視化するためにここでは Matplotlib の mplot3d を用いました。

Uploaded Image

Uploaded Image

Uploaded Image

Uploaded Image

Uploaded Image

ここではステレオキャリブレーションで用いた画像をそのまま利用しましたが、ステレオ平行化済みの画像であればその必要はありません。ステレオキャリブレーションが完了したカメラで取得した画像について、事前にステレオ平行化を行っておくことで、任意の画像について視差マップを作成できます。

Uploaded Image

ただし、特徴点が少ない画像の場合は十分な対応点が得られないことがあります。例えば ENSENSO カメラでは二つのカメラとは別にランダムドットを照射するプロジェクタを用いて特徴点を人工的に作り出すことでこれを回避しています。

関連ページ
    PCL インストールおよびビルド (Debian9、C++)
    概要 ポイントクラウド (点群) は空間内の点 (x,y,z) の集合です。例えば LiDAR (Light Detection and Ranging、レーザースキャナ、3D スキャナ) を利用して取得できます。メッシュで物体を表現している場合と区別します。 画像には通常の RGB カラー画像だけでなく、距離情報を濃淡として保存した距離画像