はじめに

OpenCVの機能の１つに、画像の直線検出があります。
この機能を使って、カメラ映像の消失点を推定。カメラの向きの変化の把握を試みました。
ジャイロセンサを持たないカメラで、姿勢認識がどこまでできるか試したかったからです。

前提

消失点の推定にはいろんな方法があり、それぞれ得手/不得手があります。

検出直線の数が少ない画像は、直線同士の交点を求めるのが確実です。特徴的な線が必要で、対象ケースは限定的です。
検出直線の数が多い画像は、直線の傾きａと切片bの座標でグラフを描いて推定できます。
長い直線のみに限定するとノイズが減りますが、推定しづらくなります。
短い直線を含めると推定しやすいですが、ノイズも増えるため、誤りやすくもなります。

今回のやり方は、信頼できる平行直線がたくさんある画像向けです。ビルや直線道路などのある街中など。逆に自然風景は苦手です。

工夫したこと

画像から検出した直線から消失点を求めるのに、直線の傾きaと切片bの座標でグラフを描く方法を選びました。なるべくいろんな画像で使えるようにするためにです。
$(a, b)$ のグラフを $(1/a, b/a)$ と逆数で描きました。正面に消失点が来ても計算しやすいようにです。
$(1/a, b/a)$ グラフで直線検出しやすいよう、スケーリングするようにしました。
消失点を求めるのに $(1/a, b/a)$ グラフ内の直線を探すのですが、ここでも画像の直線検出を使いました。
2つ目の消失点を求める前に、1つ目の消失点を求めるのに使用した点を消す処理を入れました。2つ目を見つけやすくするためです。

内容

1. PC環境

　実施したPC環境は以下のとおりです。


CPU	Celeron N4100
メモリ	8GB LPDDR4

2. 前準備

　Windowsで使えるようにするため、以下のツール / システムをインストールしました。
インストール時に参考にしたサイトも記載します。

python（使用したのはVer.4.7.0.72 ）
　実行時のベースシステムです。
（参考）https://qiita.com/ssbb/items/b55ca899e0d5ce6ce963
pip（使用したのはVer.21.2.4 ）
　他のツールをダウンロードする際に使うツールです。
（python3系ではバージョン3.4以降であれば、pythonのインストールと共にpipもインストールされます。）
（参考）https://gammasoft.jp/python/python-library-install/
OpenCV（使用したのはVer.4.5.3 ）
　画像系処理するためのライブラリです。
（参考）https://qiita.com/ideagear/items/3f0807b7bde05aa18240

3. pyファイルの作成

組んだコードは次のとおりです。長いです。

import sys
import cv2
import numpy as np

from PIL import Image

#-- 2点から傾きと切片を求める関数
def linear(n1, n2):
  if n1[0] == n2[0]:
      return [0, n1[0]]
  a = (n2[1]-n1[1])/(n2[0]-n1[0])
  b = (n2[0]*n1[1]-n1[0]*n2[1])/(n2[0]-n1[0])

  return a, b

#-- rhoとthetaから途中計算する関数
def calc_line(theta, rho):
  a0 = np.cos(theta)
  b0 = np.sin(theta)
  x0 = a0 * rho
  y0 = b0 * rho

  return -a0/b0*x0-y0, -a0/b0 

#-- rhoとthetaから直線を描く関数
def draw_line(img1, img2, theta, rho):
  a0 = np.cos(theta)
  b0 = np.sin(theta)
  x0 = a0 * rho
  y0 = b0 * rho
  x1 = int(x0 + 1000*(-b0))
  y1 = int(y0 + 1000*(a0))
  x2 = int(x0 - 1000*(-b0))
  y2 = int(y0 - 1000*(a0))

  cv2.line(img1, (x1, y1), (x2, y2), (0, 0, 255), 1)
  cv2.line(img2, (x1, y1), (x2, y2), 0, 10)

#-- メインルーチン
if __name__ == "__main__":

  length_threshold = 60 # 30 ～ 100
  distance_threshold = 1.41421356

  canny_th1 = 50
  canny_th2 = 50
  canny_aperture_size = 3
  do_merge = True

#-- 数値表示桁数の設定
  np.set_printoptions(precision=3)

#-- 映像元を設定
#  camera = cv2.VideoCapture(0)                # カメラCh.(ここでは0)を指定 <-- レスポンスまで結構時間がかかる
  camera = cv2.VideoCapture("./M6.mp4")                # 動画を指定


#-- Enterキーを押すまで一時停止
  a = input("hit enter key")

#-- ループ処理 開始
  while True:

#--  フレームを取得
    ret, img1 = camera.read()              

#--  フレームが取れなかったらループを抜ける
    if ret == False:
        break

#-- gray化 カラーのままだとエラーするため
    img = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY) 

#-- 画像サイズを調整 大きすぎるとエラーするため
    pil_img1 = Image.fromarray(img)
    rat = int((pil_img1.height + pil_img1.width) / 2000 + 1)
    (width, height) = (pil_img1.width // rat, pil_img1.height // rat)
    small_img = cv2.resize(img, dsize = (width, height))

#-- 直線検出 FastLineDetect内で使うCanny変換のパラメータ計算
    med_val = np.median(small_img)
    sigma = 0.33  # 0.33
    min_val = int(max(0, (1.0 - sigma) * med_val))
    max_val = int(max(255, (1.0 + sigma) * med_val))

    canny_th1 = min_val
    canny_th2 = max_val

#-- 直線検出関数を定義
    fld = cv2.ximgproc.createFastLineDetector(
        length_threshold,
        distance_threshold,
        canny_th1,
        canny_th2,
        canny_aperture_size,
        do_merge
    )

#-- 画像内の直線検出
    lines = fld.detect(small_img) #grayImageでないと動かないらしい

#-- 検出直線を画像に追記した画像を作成
    out = fld.drawSegments(small_img, lines)

#-- 検出直線の傾きaと切片bから（1/a, b/a）の点列tmplistを作成
    tmplist = []
    if lines is not None:
        for line in lines:
            ai, bi = linear([line[0][0],line[0][1]],[line[0][2],line[0][3]])
            if ai!= 0:
                tmplist.append([1/ai, bi/ai])

#-- tmplistをnumpy.array ablistに変換
        ablist = np.array(tmplist)

#-- ablistを（1/a, b/a）平面の画像にプロット
        maxa, maxb = ablist.max(axis=0)
        mina, minb = ablist.min(axis=0)

        if mina<maxa and minb<maxb :
            ta = 260/(maxa-mina)
            tb = 260/(maxb-minb)

            imaxa = int(maxa*ta)
            imina = int(mina*ta)
            imaxb = int(maxb*tb)
            iminb = int(minb*tb)

            abimg = np.zeros((imaxb-iminb+1,imaxa-imina+1),dtype = np.uint8)

            for a2, b2 in ablist:
                abimg[int(b2*tb)-iminb, int(a2*ta)-imina] = 255

#-- プロットした画像を複製
            abimg2 = cv2.cvtColor(abimg, cv2.COLOR_GRAY2RGB)
            abimg3 = abimg.copy()

#--- 1つめの消失点 推定-----------------------------------------------------------------
            ablines = cv2.HoughLines(abimg3, rho=0.5, theta=np.pi/360, threshold=1)
            if ablines is not None:
              for abline in ablines[:1]:
                  vpx, vpy = calc_line(abline[0][1],abline[0][0])
                  draw_line(abimg2, abimg3, abline[0][1],abline[0][0])
                  #消失点座標(x,y)
                  yt = ta/tb*vpy
                  xt = 1/tb*vpx+ta/tb*vpy*imina/ta-iminb/tb
                  #消失点の方位角と仰角を計算
                  xs1 = np.arctan((xt*2*rat-pil_img1.width)/(pil_img1.width) )*180/np.pi
                  ys1 = np.arctan((yt*2*rat-pil_img1.height)/(pil_img1.height) )*180/np.pi

#--- 2つめの消失点 推定-----------------------------------------------------------------
            ablines = cv2.HoughLines(abimg3, rho=0.5, theta=np.pi/360, threshold=1)
            if ablines is not None:
              for abline in ablines[:1]:
                  vpx, vpy = calc_line(abline[0][1],abline[0][0])
                  draw_line(abimg2, abimg3, abline[0][1],abline[0][0])
                  #消失点座標(x,y)
                  yt = ta/tb*vpy
                  xt = 1/tb*vpx+ta/tb*vpy*imina/ta-iminb/tb
                  #消失点から方位角と仰角を計算
                  xs2 = np.arctan((xt*2*rat-pil_img1.width)/(pil_img1.width) )*180/np.pi
                  ys2 = np.arctan((yt*2*rat-pil_img1.height)/(pil_img1.height) )*180/np.pi

                  #消失点の方位角と仰角の並びを揃える
                  if xs1<xs2 :
                       print(np.array([ys1,xs1,ys2,xs2]))
                  else :
                       print(np.array([ys2,xs2,ys1,xs1]))

#--           （1/a, b/a）平面の画像の検出直線の表示
#            cv2.imshow('ab_image2',abimg2)

#--            検出直線の画像表示
            cv2.imshow('detected',out)

    # キー操作があればwhileループを抜ける
            if cv2.waitKey(1) & 0xFF == ord('q'):
              break
#--  ループ処理 ここまで

#--  終了処理
  camera.release()
  cv2.destroyAllWindows()

4. 説明

1. 検出直線の傾きaと切片bから消失点を計算する方法

消失点を求める理屈の説明です。$n$ 個の直線のうち $i$ 番目の直線の式は
$$y=a_{i}x+b_{i}$$で表せます。それらの直線がすべて同じ消失点 $(x_{0}, y_{0})$ を通るならば、
$$y_{0}=a_{i}x_{0}＋b_{i}$$が成り立ちます。式を変形すれば
$$b_{i}＝-x_{0}a_{i}+y_{0}$$と表せ、そのため $(a,b)$ のグラフに $(a_{i}, b_{i})$ をプロットすれば、それらの点は傾き $(-x_{0})$、切片$y_{0}$ の直線上に並ぶはずです。
この直線を求めることで消失点 $(x_{0}, y_{0})$ がわかります。

2. (a, b)の代わりに(1/a, b/a)を使うことのメリット/デメリット

i番目の直線を表す式
$$y=a_{i}x+b_{i}$$を変形すれば
$$b_{i}/a_{i}=y(1/a_{i})-x$$
と表せ、$(1/a, b/a)$ のグラフに $(1/a_{i}, b_{i}/a_{i})$ をプロットすれば、先ほどと同じ理屈で、それらの点は傾き $y_{0}$ 、切片 $(-x_{0})$ の直線上に並ぶはずです。
この直線を求めることでも消失点 $(x_{0}, y_{0})$ が求まります。

$(a, b)$ の代わりに $(1/a, b/a)$ を使うメリットは、真っすぐ垂直な直線を扱えることです。逆にデメリットは真っすぐ水平な直線を扱えないことです。
もう1つ。一般的な画像であれば水平線はだいたい画像に水平になるため、水平線上の2つの消失点 $(x_{0}^{1}, y_{0}^{1}), (x_{0}^{2}, y_{0}^{2})$ は
$$y_{0}^{1}=y_{0}^{2}$$ $$x_{0}^{1} \neq x_{0}^{2}$$ となり、 $(1/a, b/a)$ のそれぞれを表す直線は平行な別の直線になり見分けやすくなるはずと考えました。

3. グラフのスケーリング

$(1/a, b/a)$ の配列ですが、配列が小さすぎるとプロットがつぶれて傾きが求められなくなります。また、配列が大きくなると HoughLines で読み込めなくなります。
配列が $260 \times 260$ 程度になるようスケーリングして使用してます。

        maxa, maxb = ablist.max(axis=0)
        mina, minb = ablist.min(axis=0)

        if mina<maxa and minb<maxb :
            ta = 260/(maxa-mina)
            tb = 260/(maxb-minb)

            imaxa = int(maxa*ta)
            imina = int(mina*ta)
            imaxb = int(maxb*tb)
            iminb = int(minb*tb)

4. グラフの直線検出

$(1/a, b/a)$ グラフの直線検出のため、numpy.zerosの配列を作り $(1/a_{i}, b_{i}/a_{i})$ のところだけ255に値を変更、その配列を直線検出関数 HoughLines にかけてます。

            abimg = np.zeros((imaxb-iminb+1,imaxa-imina+1),dtype = np.uint8)

            for a2, b2 in ablist:
                abimg[int(b2*tb)-iminb, int(a2*ta)-imina] = 255

            ablines = cv2.HoughLines(abimg3, rho=0.5, theta=np.pi/360, threshold=1)

5. 使用した点の削除

プロット点から直線検出してるため、同じ点列から複数の直線を推定してしまいます。
それを避けるため、1つ目の直線を検出後、その直線を幅 10pt の太線で描き、プロットを消すようにしました。

  cv2.line(img2, (x1, y1), (x2, y2), 0, 10)

5. 結果

　実カメラでも試しましたが、CG映像を作成して、検出の傾向がわかるようにして試しました。
トライで使った映像は以下のようなものです。

検出結果は例えば、以下の画像のようになりました。

検出の傾向として、以下のようなものが見られました。

消失点が画像の左右にあるようなケースでは±1度くらいの精度で認識することができました。
消失点が正面、また左右の遠く離れたところにあるケースでは、精度が出ず、答えが安定しませんでした。

参考

参考サイト
直線検出方法と消失点のアイデア）
https://nsr-9.hatenablog.jp/entry/2021/08/12/200000

消失点の検出方法）
http://opt.imi.kyushu-u.ac.jp/~fujisawa/yc_detect.pdf

Canny法のパラメータ）
https://kuroro.blog/python/wOt3yEohr7oQt1qzif71/
https://qiita.com/kotai2003/items/662c33c15915f2a8517e