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()
