【画像処理】チェッカーボードを使用せずに歪曲収差の補正係数を算出する

はじめに

　チェッカーボードとopencvのcv2.findChessboardCorners、cv2.calibrateCamera、cv2.undistortの三連コンボであっという間に歪曲収差の除去と内部パラメータの取得ができてしまう時代ではあるが、あえてこれを使わないで歪曲収差の除去をやりたい。というか、カメラがないと歪曲収差除去と内部パラメータ取得ができないとか不便すぎる。実空間上で一直線上に並んだ画像上の特徴点を選択して、その歪が小さくなるような補正係数を計算する。
　文献*1を参考に以下のように実装した。

透視投影モデル

透視投影

歪曲収差補正モデル

歪曲収差

　上図のように、歪の大きい画像と歪の小さい画像(補正された画像)上における点の位置を定義し、以下のような流れで歪曲収差を補正する。
1. 歪のない画像の画素位置を画像平面座標系から正規化座標系に変換
　
　
2. 歪のない画像の画素位置と補正係数から歪を補正した画像の画素位置を算出
　
　
　
3. 歪を補正した画素位置を正規化画像座標系から画像平面座標系に変換
　
　
　以上のように、歪曲収差を補正するにはの６つのパラメータが必要になる。歪曲収差が複雑な変形をする場合、を増やす必要はあるが、たいていの場合までで十分補正できる。

補正係数の推定

　以下の流れで補正係数を推定する。

1. 直線状に並んだ画像上の特徴点の画素位置を抽出する
2. 一定の間隔でパラメータを振って、複数のパラメータを生成する
3. 生成したパラメータ群すべてにおいて特徴点画素位置を補正する
4. 補正された特徴点位置について、それぞれ最小二乗法による近似直線を得る
5. 補正された特徴点から近似直線までの残差をそれぞれ計算する
6. パラメータ群の中で残差が小さいものを１つ選択する
7. 選択したパラメータを中心とし、振る間隔を狭めてパラメータを生成する
8. 2.に戻り、設定回数繰り返す

ソースコード

　以下のソースコードでGUIで１直線上にある特徴点を選択して補正係数を計算しcsvに保存することができる。直線は複数選択でき、特徴点の数と直線の本数が多いほど誤差が小さくなるはず。
github.com

*1:秋葉 教充, 小倉 崇生, 戸田 均, 黒沢 健至, 土屋 兼一, 黒木 健郎, 法工学鑑定のための半自動画像幾何変換プログラムの開発, 日本法科学技術学会誌, 2015, 20 巻, 2 号, p. 157-164

P3P問題を使用してドラレコ映像から車速を推定する

ドラレコ映像からの車速推定

　ドラレコ映像のみから自己車速を推定する方法は様々あるが、一般的には寸法が既知の路面標示が見切れる時間を計測することが多い。最近の自動運転関連のレポートを見ると、ステレオカメラやLiDARなどを駆使して自己車速だけでなく対向車車速などを推定しているようにある。特徴点検出など様々なハードルはあれど、画像上の特徴点間の実距離を計測できるため比較的手軽に車速を推定できる。
　ドラレコ映像のみから自己車速を推定するとき問題となるのが特徴点と特徴点間の実距離。前述した方法では、寸法が既知の特徴点が見切れるタイミングでしか車速推定できない。都合よく寸法が既知の特徴点がちょうどいい位置に映ることなんか稀なので実用性はいまいち。
　P3P問題を解くとちょうどいい位置（見切れる位置）に映らなくてもカメラ位置がわかるのでドラレコ映像の自己車速推定の適用範囲が広がる。というか、流行りのVSLAMとかSfMは、P3Pを拡張したPnP問題とかをゴリゴリ解いてるのでできるのは当たり前のことではある。研究でやってる人は何を当たり前のことを言ってるんだと思うはずではある。

実験条件

　使用したドラレコにはGPSセンサが付いていないため、自己車速の真値はUSB型のGPSセンサで取得した。GPS車速とドラレコ映像の同期は、GPSのタイムスタンプ表示をドラレコに映しこんで時差を計算して合わせた。
　計算に使用した画像は以下のような路面標示「横断歩道又は自転車横断帯あり」(以後ダイヤマーク)が映った8枚の画像である。これらの画像は事前にチェッカーボードを使用したカメラキャリブレーション方法でレンズの歪曲収差を取り除いている。

　前の記事で実装したコードの、画像位置に画像上のダイヤマークの頂点のピクセル位置、画像位置に対応する三次元空間位置にダイヤマークの実寸法を入力した。
jonajiro.hatenablog.com

実験結果

　以下に、ダイヤマークと推定されたカメラ位置を３次元上に表示した。なお、4次方程式の解はそれぞれ２候補ずつ計算されたが、ダイヤマークに対して車両がy軸負方向にあることを前提として解を１つに絞り込んだ。緑矢印が自己車両の進行方向。

　各フレーム間のカメラ移動距離をユークリッド距離で算出し、フレームレートを掛け算したものを以下の表に示す。

　1フレーム間で計算した速度はばらつきがあるが、7フレーム間の平均値は真値とのずれが1%未満であった。誤差要因は、画像上のダイヤマーク頂点の選択誤差、カメラキャリブレーションで取り除けなかった歪曲収差、フロントガラスで生じた歪み、動画フレームレート誤差などが考えられる。

応用

　対向車のナンバープレートの大きさを使用してカメラ位置を計算すれば、路面標示などの地面定着物を使用した自己位置推定と組み合わせて対向車車速の推定も可能と考えられる。でも実験がめんどくさそう。

カメラの位置・姿勢推定Perspective-3-Point問題について

Perspective-3-Point問題

　レンズの歪曲収差が補正された理想的なピンホールカメラモデルが適用できる画像において、三次元空間位置と対応する画像位置のペア３つからカメラの光学中心の位置・姿勢を計算する問題。
　カメラ光学中心から３点の画像位置に対して張った３つのベクトルの長さを調整してやれば、それぞれの画像位置に対応する三次元空間位置と重なるよねみたいなノリである。
　このベクトルの長さは四次方程式を解くことで得られるが四次方程式なので解が４つ存在する。どれが本物の解なのかは人間が画像から予想すればある程度分かる。客観的な判断方法は、４つ目の別の三次元空間位置と対応する画像位置のペアを用意し、解候補を使ってこれを画像上に再投影したときの誤差（再投影誤差）が小さい解候補が本物の答えと判断できる場合がある。しかし、４つ目のペアの選び方によっては複数の解候補で再投影誤差が小さくなり本物が正しく判断できない場合もあるので、４つ目のペアの選択基準が結構大事だったりする。

Fischler and Bollesの手法で実装してみた

FischlerとBolles*1の方法でカメラ位置を計測するプログラムを実装した。実行すると三次元空間位置が赤点、計算されたカメラ位置が緑点で表示される。以下の例の場合、カメラ位置の候補が２個あるので緑点が２個表示されていることが確認できる。カメラの撮影方向などの前提情報を使って本物の解を選択することもできるが、不明な場合はいくつかの三次元空間位置と対応する画像位置のペアを用意する必要がある。

import sys
import numpy as np
from pyqtgraph.Qt import QtCore, QtGui
import pyqtgraph.opengl as gl

def main():
    #カメラの内部パラメータ
    mtx      = np.array([[1200.0,0     ,1020.0  ],
                         [0     ,1200.0,540.0   ],
                         [0     ,0     ,1       ]])
    
    #画像位置
    img_pos  = np.array([[ 600.0 ,850.0  ],
                         [1000.0 ,850.0  ],
                         [1300.0 ,830.0]])
    
    #画像位置に対応する三次元空間位置
    real_pos = np.array([[0.7 ,-4.0 ,-0.91],
                         [1.2 ,-4.1 ,-0.91],
                         [1.6 ,-4.1 ,-0.91]])
    
    #P3Pを解く。カメラの三次元位置と実数解の個数を返す。
    cam_pos, ans_num = calcPoseP3P(img_pos,real_pos,mtx)
    cam_pos = cam_pos[0:ans_num,:]
    print("Camera Position")
    print(cam_pos)

    color_data0 = np.zeros([len(real_pos),3])
    for i in range(len(real_pos)):
        color_data0[i,0] = 1

    color_data1 = np.zeros([len(cam_pos),3])
    for i in range(len(cam_pos)):
        color_data1[i,1] = 1

    point_data = np.vstack([real_pos,cam_pos])
    color_data = np.vstack([color_data0,color_data1])
  
    v = PlotObject()
    v.plotGLPlot(point_data,color_data)
    v.show()
    sys.exit(v.App.exec_())

def calcPoseP3P(pixel,pos,mtx):
    fx = mtx[0,0]
    fy = mtx[1,1]
    Cu = mtx[0,2]
    Cv = mtx[1,2]

    q = np.zeros([len(pixel),3])
    for i in range(len(pixel)):
        q[i,0] = (pixel[i,0] - Cu)/fx
        q[i,1] = (pixel[i,1] - Cv)/fy
        q[i,2] = 1
        q[i,:] = q[i,:]/np.linalg.norm(q[i,:])
    a = np.linalg.norm(pos[0,:]-pos[1,:])
    b = np.linalg.norm(pos[1,:]-pos[2,:])
    c = np.linalg.norm(pos[2,:]-pos[0,:])
    ca = q[0,:]@q[1,:]
    cb = q[1,:]@q[2,:]
    cc = q[2,:]@q[0,:]
    ca2 = ca*ca
    cb2 = cb*cb
    cc2 = cc*cc
    B4 = 4*b**2*c**2*ca2 - (a**2-b**2-c**2)**2
    B3 = -4*c**2*(a**2+b**2-c**2)*ca*cb - 8*b**2*c**2*ca2*cc + 4*(a**2-b**2-c**2)*(a**2-b**2)*cc
    B2 = 4*c**2*(a**2-c**2)*cb2 + 8*c**2*(a**2+b**2)*ca*cb*cc + 4*c**2*(b**2-c**2)*ca2 - 2*(a**2-b**2-c**2)*(a**2-b**2+c**2) - 4*(a**2-b**2)**2*cc2
    B1 = -8*a**2*c**2*cb2*cc - 4*c**2*(b**2-c**2)*ca*cb - 4*a**2*c**2*ca*cb + 4*(a**2-b**2)*(a**2-b**2+c**2)*cc
    B0 = 4*a**2*c**2*cb2 - (a**2-b**2+c**2)**2

    s = SolveQuarticEquation(B4,B3,B2,B1,B0)
    ans_num = 0
    cam_pos = np.zeros([4,3],dtype="float32")


    for i in range(len(s)):
        if np.abs(s[i].imag) < 0.00000001:
            u = s[i].real
            v = -((a**2-b**2-c**2)*u**2+2*(b**2-a**2)*cc*u+(a**2-b**2+c**2))/(2*c**2*(ca*u-cb))
            x = a * a / (u * u + v * v - 2.0 * u * v * ca)
            y = b * b / (1 + v * v - 2.0 * v * cb)
            z = c * c / (1 + u * u - 2.0 * u * cc)
            if (x > 0.0):
                s1 = np.sqrt(x)
                s2 = u * s1
                s3 = v * s1
                P1 = q[0,:]*s2
                P2 = q[1,:]*s3
                P3 = q[2,:]*s1
                P12 = P2-P1
                P13 = P3-P1
                P23 = P3-P2
                zv = np.array(np.cross(P12,P13),dtype="float32")
                zv = zv/np.linalg.norm(zv)
                xv = np.array(P12,dtype="float32")
                xv = xv/np.linalg.norm(xv)
                yv = np.array(np.cross(xv,zv),dtype="float32")
                yv = yv/np.linalg.norm(yv)
                Rotc = np.zeros([3,3])
                Rotc[:,0] = xv
                Rotc[:,1] = yv
                Rotc[:,2] = zv
                P12b = pos[1,:]-pos[0,:]
                P13b = pos[2,:]-pos[0,:]
                zb = np.array(np.cross(P12b,P13b),dtype="float32")
                zb = zb/np.linalg.norm(zb)
                xb = np.array(P12b,dtype="float32")
                xb = xb/np.linalg.norm(xb)
                yb = np.array(np.cross(xb,zb),dtype="float32")
                yb = yb/np.linalg.norm(yb)
                Rota = np.zeros([3,3])
                Rota[:,0] = xb
                Rota[:,1] = yb
                Rota[:,2] = zb

                t = Rota@Rotc.T@(-P1.T)+pos[0,:].T
                cam_pos[ans_num,:] = t
                ans_num = ans_num + 1

    return cam_pos , ans_num

def SolveCubicEquation(a,b,c,d):
    if a == 0.0:
        return 0
    else:
        A = b/a
        B = c/a
        C = d/a
        p = B-A*A/3.0
        q = 2.0*A*A*A/27.0-A*B/3.0+C
        D = q*q/4.0+p*p*p/27.0
        if D < 0.0:
            x = np.zeros(3)
            theta = np.arctan2(np.sqrt(-D),-q*0.5);
            x[0] = 2.0*np.sqrt(-p/3.0)*np.cos(theta/3.0)-A/3.0
            x[1] = 2.0*np.sqrt(-p/3.0)*np.cos((theta+2.0*np.pi)/3.0)-A/3.0
            x[2] = 2.0*np.sqrt(-p/3.0)*np.cos((theta+4.0*np.pi)/3.0)-A/3.0
        else:
            x = np.zeros(3,dtype=complex)
            u = Cuberoot(-q*0.5+np.sqrt(D))
            v = Cuberoot(-q*0.5-np.sqrt(D))
            x[0] = u+v-A/3.0
            x[1] = complex(-0.5*(u+v)-A/3.0,np.sqrt(3.0)*0.5*(u-v))
            x[2] = complex(-0.5*(u+v)-A/3.0,-np.sqrt(3.0)*0.5*(u-v))
        return x

def SolveQuarticEquation(a,b,c,d,e):
    if a == 0.0:
        return 0
    else:
        x = np.zeros(4,dtype=complex)
        A = b/a
        B = c/a
        C = d/a
        D = e/a
        p = -6.0*(A/4.0)**2.0+B
        q = 8.0*(A/4.0)**3.0-2.0*B*A/4.0+C
        r = -3.0*(A/4.0)**4.0+B*(A/4.0)**2.0-C*A/4.0+D
        t_temp = SolveCubicEquation(1.0,-p,-4.0*r,4.0*p*r-q*q)
        t = t_temp[0].real
        m = Squreroot(t-p)
        x[0] = (-m+Squreroot(-t-p+2.0*q/m))*0.5-A/4.0
        x[1] = (-m-Squreroot(-t-p+2.0*q/m))*0.5-A/4.0
        x[2] = (m+Squreroot(-t-p-2.0*q/m))*0.5-A/4.0
        x[3] = (m-Squreroot(-t-p-2.0*q/m))*0.5-A/4.0
        return x

def Cuberoot(x):
    if x > 0.0:
        return (x)**(1.0/3.0)
    else:
        return -(-x)**(1.0/3.0)
def Squreroot(x):
    r = np.sqrt(x.real*x.real+x.imag*x.imag)
    theta = np.arctan2(x.imag,x.real)
    if x.imag == 0.0:
        if x.real > 0.0:
            y = np.sqrt(r)
        else:
            y = complex(0,np.sqrt(r))
    else:
        if theta < 0.0:
            theta = theta + 2.0*np.pi
        y = complex(np.sqrt(r)*np.cos(theta*0.5),np.sqrt(r)*np.sin(theta*0.5))
    return y

class PlotObject(gl.GLViewWidget):
    sigUpdate = QtCore.Signal(float, float)
    App = None
    def __init__(self, app=None):

        if self.App is None:
            if app is not None:
                self.App = app
            else:
                self.App = QtGui.QApplication([])
        super(PlotObject,self).__init__()

        self.Gridxy = gl.GLGridItem()
        self.Axes = gl.GLAxisItem()
        self.Gridxy.setSize(100,100,3)
        self.Gridxy.setSpacing(1,1,0)
        self.Gridxy.translate(0, 0, 0)

        self.Poss = []
        self.Cols = []
        self.Sizes = []
        self.GlobalInds = []

        self.Plot = gl.GLScatterPlotItem()

        self.addItem(self.Plot)
        self.addItem(self.Gridxy)

        self.addItem(self.Axes)
        self._downpos = []

        self.setWindowTitle('View 3D points')

    def mousePressEvent(self, ev):
        super(PlotObject, self).mousePressEvent(ev)
        self._downpos = self.mousePos

    def mouseReleaseEvent(self, ev):
        super(PlotObject, self).mouseReleaseEvent(ev)
        self._prev_zoom_pos = None
        self._prev_pan_pos = None

    def plotGLPlot(self,pos_input,color_input):
        pos_data = np.asarray(pos_input)
        length = len(pos_data)
        colors = [None for k in range(length)]
        poss = np.array([0,0,0])
        cols = np.array([0,0,0,0])
        for k in range(length):
            rgb_val = np.zeros(3,dtype=float)
            rgb_val[0] = color_input[k,0]
            rgb_val[1] = color_input[k,1]
            rgb_val[2] = color_input[k,2]
            colors[k] = (rgb_val[0],rgb_val[1],rgb_val[2])
            self.Poss.append([pos_data[k,0], pos_data[k,1], pos_data[k,2]])
            self.Cols.append([rgb_val[0], rgb_val[1], rgb_val[2]])
            self.Sizes.append(1)
            self.GlobalInds.append(k)
            possb = np.array([pos_data[k,0], pos_data[k,1], pos_data[k,2]])
            poss = np.vstack([poss,possb])
            colsb = np.array([rgb_val[0], rgb_val[1], rgb_val[2],1])
            cols = np.vstack([cols,colsb])

        self.Plot = gl.GLScatterPlotItem()
        self.Plot.setData(pos=poss, size=0.05, color=cols, pxMode=False)

        self.addItem(self.Plot)
        self.show()

if __name__ == '__main__':
    main()

*1:M.A. Fischler and R.C. Bolles, “Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartography,”Commun. ACM, vol.24, no.6, pp.381–395, June 1981.