な
写真測量 (コンピューター ビジョン) における三角測量法
三角形 測量 CV の 分野 で は 画像 の 点 から 物体 の 点 を 計算 する プロセス を 三角 測 量 と 呼び ます が、 写真 測量 の 分野 で は 前方 交差 と 呼ばれ ます。1 つの画像ではピクセルの 3D 座標を復元できないことに注意してください。ピクセルの実際の座標を取得するには、少なくとも 2 つの画像が必要です (2 つの画像の姿勢情報はここで既知です)。
三角測量にはいろいろな方法がありますが、ここでは2フレーム三角測量、マルチフレーム三角測量、反復三角測量、重み選択反復マルチフレーム三角測量(私のコードを添付)を紹介します。
1. 2 フレームの三角形分割
opencv では、関数 triangulatePoints は、2 つのフレームのポーズと内部パラメーターに従って、3 次元のポイント座標を復元できます.cv の三角形分割は 2 つのフレームであり、権利はありません。
関数パラメータは次のとおりです。
void cv::triangulatePoints ( InputArray projMatr1, //P1 1 3*4
InputArray projMatr2, //P2 3*4
InputArray projPoints1, //pixel coordinates
InputArray projPoints2, // pixel coordinates
OutputArray points4D // 3D coordinates
)
2. マルチフレーム三角測量
三角化严密解推导過程:
x − x 0 = − fa 1 ( X − X s ) + b 1 ( Y − Y s ) + c 1 ( Z − Z s ) a 3 ( X − X s ) + b 3 ( Y − Y s ) + c 3 ( Z − Z s ) y − y 0 = − fa 2 ( X − X s ) + b 2 ( Y − Y s ) + c 2 ( Z − Z s ) a 3 ( X − X s ) + b 3 ( Y − Y s ) + c 3 ( Z − Z s ) \begin{array}{l} x-x_{0}=-f \frac{a_{1}\left( X-X_{s}\right)+b_{1}\left(Y-Y_{s}\right)+c_{1}\left(Z-Z_{s}\right)}{a_{3}\ left(X-X_{s}\right)+b_{3}\left(Y-Y_{s}\right)+c_{3}\left(Z-Z_{s}\right)} \\ y- y_{0}=-f \frac{a_{2}\left(X-X_{s}\right)+b_{2}\left(Y-Y_{s}\right)+c_{2}\left (Z-Z_{s}\right)}{a_{3}\left(X-X_{s}\right)+b_{3}\left(Y-Y_{s}\right)+c_{3} \left(Z-Z_{s}\right)} \end{配列}バツ−バツ0=− fa3( X − Xs) + b3( Y − Ys) + c3( Z − Zs)a1( X − Xs) + b1( Y − Ys) + c1( Z − Zs)y−y0=− fa3( X − Xs) + b3( Y − Ys) + c3( Z − Zs)a2( X − Xs) + b2( Y − Ys) + c2( Z − Zs)な
共線条件式から得られる三角化秘密法は、分母を左边に移し、
( x − x 0 ) [ a 3 ( X − X s ) + b 3 ( Y − Y s ) + c 3 ( Z − Z 5 ) ] = − f [ a 1 ( X − X s ) + b 1 ( Y − Y s ) + c 1 ( Z − Z s ) ] ( y − y 0 ) [ a 3 ( X − X s ) + b 3 ( Y − Y s ) + c 3 ( Z − Z s ) ] = − f [ a 2 ( X − X s ) + b 2 ( Y − Y s ) + c 2 ( Z − Z s ) ] \begin {array}{c} \left(x-x_{0}\right)\left[a_{3}\left(X-X_{s}\right)+b_{3}\left(Y-Y_{s }\right)+c_{3}\left(Z-Z_{5}\right)\right]= \\ -f\left[a_{1}\left(X-X_{s}\right)+b_ {1}\left(Y-Y_{s}\right)+c_{1}\left(Z-Z_{s}\right)\right] \\ \left(y-y_{0}\right)\ left[a_{3}\left(X-X_{s}\right)+b_{3}\left(Y-Y_{s}\right)+c_{3}\left(Z-Z_{s}\ right)\right]= \\ -f\left[a_{2}\left(X-X_{s}\right)+b_{2}\left(Y-Y_{s}\right)+c_{2 }\left(Z-Z_{s}\right)\right] \end{配列}( ×−バツ0)[ _3( X−バツs)+b3(や−よs)+c3( Z−Z5) ]=− f[ _1( X−バツs)+b1(や−よs)+c1( Z−Zs) ]( _−y0)[ _3( X−バツs)+b3(や−よs)+c3( Z−Zs) ]=− f[ _2( X−バツs)+b2(や−よs)+c2( Z−Zs) ]
整理可能:
l 1 X + l 3 Y + l 3 Z − lx = 0 L 4 X + l 5 Y + l 6 Z − ly = 0 \begin{array}{l} l_{1} X+l_{ 3} Y+l_{3} Z-l_{x}=0 \\ L_{4} X+l_{5} Y+l_{6} Z-l_{y}=0 \end{配列}l1バツ+l3よ+l3Z−l×=0L4バツ+l5よ+l6Z−ly=0
その中:
I 1 = fa 1 + ( x − x 0 ) a 3 I_{1}=f a_{1}+\left(x-x_{0}\right) a_{3}私1=ファ_1+( ×−バツ0)a3
l 2 = fb 1 + ( x − x 0 ) b 3 l_{2}=f b_{1}+\left(x-x_{0}\right) b_{3}l2=f b1+( ×−バツ0)b3
l 3 = fc 1 + ( x − x 0 ) c 3 l_{3}=f c_{1}+\left(x-x_{0}\right) c_{3}l3=f c1+( ×−バツ0)c3
lx = fa 1 X s + fb 1 Y s + fc , Z s + ( x − xb ) a 3 X s + ( x − x 0 ) b 3 Y s + ⟨ x − xb ) c 3 Z s l_{x }=f a_{1} X_{s}+f b_{1} Y_{s}+f_{c}, Z_{s}+\left(x-x_{b}\right) a_{3} X_{ s}+\left(x-x_{0}\right) b_{3} Y_{s}+\left\langle x-x_{b}\right) c_{3} Z_{s}l×=ファ_1バツs+f b1よs+へc、Zs+( ×−バツb)a3バツs+( ×−バツ0)b3よs+⟨ ×−バツb)c3Zs
l 4 = fa 2 + (y − y 0 ) a 3 l_{4}=f a_{2}+\left(y-y_{0}\right) a_{3}l4=ファ_2+( _−y0)a3
L 5 = fb 2 + (y − y 0 ) b 3 L_{5}=f b_{2}+\left(y-y_{0}\right) b_{3}L5=f b2+( _−y0)b3
l 6 = fc 2 + (y − y 0 ) c 3 l_{6}=f c_{2}+\left(y-y_{0}\right) c_{3}l6=f c2+( _−y0)c3
ly = fa 2 X s + fb 2 Y s + fc 2 Z s + (y − y 0 ) a 3 X s + (y − y 0 ) b 3 Y s + (y − y 0 ) c 3 Z s l_ {y}=f a_{2} X_{s}+f b_{2} Y_{s}+f c_{2} Z_{s}+\left(y-y_{0}\right) a_{3} X_{s}+\left(y-y_{0}\right) b_{3} Y_{s}+\left(y-y_{0}\right) c_{3} Z_{s}ly=ファ_2バツs+f b2よs+f c2Zs+( _−y0)a3バツs+( _−y0)b3よs+( _−y0)c3Zs
上記から、1 つのピクセルは 2 つの方程式をリストでき、同じ名前の n ピクセルに対しては 2n の方程式をリストできることがわかります (AX=B、過決定方程式は最小二乗法に従って解かれます)。フレーム三角形分割、コードは次のとおりです。
def pixelToCam(pts,K):
'''
:param pts: pixel coordinates
:param K: camera params
:return: camera coordinates
'''
camPts=np.zeros((1,2))
camPts[0,0]=(pts[0,0]-K[0,2])/K[0,0]
camPts[0,1]=(pts[0,1]-K[1,2])/K[1,1]
return camPts
def getEquation(camPts,R,t):
'''
build equation ,one pixel point get 2 equations
:param camPts: camera coordinates
:param R: image pose-rotation ,world to camera
:param t: image pose -translation,is camera center(t=-R.T*tvec)
:return: equation coefficient
'''
A=np.zeros((2,3))
b=np.zeros((2,1))
A[0,0]=R[0,0]-camPts[0,0]*R[2,0]
A[0,1]=R[0,1]-camPts[0,0]*R[2,1]
A[0,2]=R[0,2]-camPts[0,0]*R[2,2]
b[0,0]=t[0,0]*A[0,0]+t[0,1]*A[0,1]+t[0,2]*A[0,2]
A[1,0]=R[1,0]-camPts[0,1]*R[2,0]
A[1,1]=R[1,1]-camPts[0,1]*R[2,1]
A[1,2]=R[1,2]-camPts[0,1]*R[2,2]
b[1,0]=t[0,0]*A[1,0]+t[0,1]*A[1,1]+t[0,2]*A[1,2]
return A,b
3. 反復三角測量
方法は、方程式の係数に係数を追加し、係数の係数を継続的に調整することです. 私のコードは次のとおりです:
def IterativeLinearLSTri(u1,P1,u2,P2):
wi,wi1=1,1 #不断需要更新的因子
X=np.zeros((4,1))
for i in range(10): #迭代10次
X_=linear_ls_triangulation(u1,P1,u2,P2) # 线性求解两帧的像素点的三维坐标
X[1,0]=X_[0,1]
X[2,0]=X_[0,2]
X[3,0]=1
p2x=np.dot(P1[2,:].reshape(1,4),X)[0,0]
p2x1=np.dot(P2[2,:].reshape(1,4),X)[0,0]
if abs(wi-p2x) <=0.001 and abs(wi1-p2x1)<=0.001 :
break
wi=p2x
wi1=p2x1
A = np.array([(u1[0]*P1[2, 0] - P1[0, 0])/wi, (u1[0]*P1[2, 1] - P1[0, 1])/wi,
(u1[0]*P1[2, 2] - P1[0, 2])/wi, (u1[1]*P1[2, 0] - P1[1, 0])/wi,
(u1[1]*P1[2, 1] - P1[1, 1])/wi, (u1[1]*P1[2, 2] - P1[1, 2])/wi,
(u2[0]*P2[2, 0] - P2[0, 0])/wi1, (u2[0]*P2[2, 1] - P2[0, 1])/wi1,
(u2[0]*P2[2, 2] - P2[0, 2])/wi1, (u2[1]*P2[2, 0] - P2[1, 0])/wi1,
(u2[1]*P2[2, 1] - P2[1, 1])/wi1,
(u2[1]*P2[2, 2] - P2[1, 2])/wi1]).reshape(4, 3)
B = np.array([-(u1[0]*P1[2, 3] - P1[0, 3])/wi,
-(u1[1]*P1[2, 3] - P1[1, 3])/wi,
-(u2[0]*P2[2, 3] - P2[0, 3])/wi1,
-(u2[1]*P2[2, 3] - P2[1, 3])/wi1]).reshape(4, 1)
ret, X_ = cv2.solve(A, B, flags=cv2.DECOMP_SVD)
X[0,0]=X_[0,0]
X[1,0]=X_[1,0]
X[2,0]=X_[2,0]
X[3,0]=1
return X
4. 重み選択反復マルチフレーム三角測量
最初に重み選択反復法 (IGG アルゴリズム) について説明します. 1980 年代に、事後分散推定から導き出されて総誤差を特定する最初の重み選択反復法は、「Li Deren 法」と名付けられました。実際の測定作業における客観的な条件の制限により、全体的なエラーの存在を完全に回避したり、同じ測定精度を達成したりすることは困難です.調整の過程で、通常、相対的な比較のための指標として重量が導入されます.精度の高い観測データほど重み付けを高くすることで、有害な情報の干渉を避けることができます。たとえば、画像マッチングを照合するときは、ransac (これも堅牢な推定アルゴリズム) を使用して外れ値を排除しますが、同じ名前のポイントが複数のフレームにあり、1 つしかない場合 (複数のフレームの位置測定など)フレーム信号) , ransac はもはや使用できません. 現時点では, IGG アルゴリズムの使用はエラーの影響を効果的に回避することができます. この論文は, 複数画像空間前方交差のロバストな総最小二乗推定に言及しています.論文のアルゴリズムを再現したコードは次のとおりです。
def IterationInsection(pts,K,R,t):
# cam_xyz is inital value
# 这里假设像点x,y是等权的
k0=1.5
k1=2.5 # K1=2
weight=np.identity(len(pts)*2)
cam_xyz=mutiTriangle(pts,K,R,t)
cam_xyz_pre=cam_xyz
iteration=0
while 1:
d=np.zeros((len(pts),1))
for i in range(len(R)):
pro,J = cv2.projectPoints(cam_xyz.reshape(1, 1, 3), R[i], t[i], K, np.array([]))
pro=pro.reshape(1,2)
deltax=pro[0,0]-pts[i][0,0]
deltay=pro[0,1]-pts[i][0,1]
d[i,0]=np.sqrt(deltax**2+deltay**2)
weight_temp=np.diag(weight)[::2].reshape(-1,1)
delta=np.sqrt(np.sum(weight_temp*d**2)/(len(pts)-2))
w=np.zeros((len(pts),1))
for i in range(len(pts)):
u=d[i]
if abs(u)<k0*delta:
w[i]=1
elif abs(u)<k1*delta and abs(u)>=k0*delta:
w[i]=delta/u
elif abs(u)>=k1*delta:
w[i]=0
weight_temp=w
weight_p=[val for val in weight_temp.reshape(-1,) for i in range(2)]
weight_p=np.diag(weight_p)
cam_xyz_curr=weight_mutiTriangle(pts,K,R,t,weight_p)
dx=cam_xyz_curr[0,0]-cam_xyz_pre[0,0]
dy=cam_xyz_curr[1,0]-cam_xyz_pre[1,0]
dz=cam_xyz_curr[2,0]-cam_xyz_pre[2,0]
# print(dx,dy,dz)
if np.sqrt(dx**2+dy**2+dz**2)<0.01:
break
else:
cam_xyz=cam_xyz_curr
cam_xyz_pre=cam_xyz_curr
weight=weight_p
iteration+=1
# print("d{0}".format(d))
print("iteration is {0}\n".format(iteration))
print("IGG....{0},{1},{2}".format(cam_xyz[0,0],cam_xyz[1,0],cam_xyz[2,0]))
return cam_xyz,weight
その中で、mutiTriangle 関数と weight_mutiTriangle 関数は次のとおりです。
def mutiTriangle(pts,K,R,t):
if len(pts)>=4: #这里是假设至少track 4帧
equa_A=[]
equa_b=[]
for i in range(len(pts)):
camPts=pixelToCam(pts[i],K)
t1=np.dot(np.linalg.inv(R[i]),-t[i]).reshape(1,3)
A1,b1=getEquation(camPts,R[i],t1)
equa_A.append(A1)
equa_b.append(b1)
AA=np.vstack(equa_A)
bb=np.vstack(equa_b)
P_ls=np.dot(np.linalg.inv(AA.T@AA),AA.T@bb)
return P_ls
else:
print("tracker pixel point less 3,can not insection........")
return None
def weight_mutiTriangle(pts,K,R,t,weight):
if len(pts)>=4:
equa_A=[]
equa_b=[]
for i in range(len(pts)):
camPts=pixelToCam(pts[i],K)
t1=np.dot(np.linalg.inv(R[i]),-t[i]).reshape(1,3)
A1,b1=getEquation(camPts,R[i],t1)
equa_A.append(A1)
equa_b.append(b1)
AA=np.vstack(equa_A)
bb=np.vstack(equa_b)
P_ls=np.dot(np.linalg.pinv(AA.T@weight@AA),AA.T@weight@bb)
return P_ls
else:
print("tracker pixel point less 4,can not insection........")
return None
参照:
1. 複数の画像空間での前方交差に対するロバストな総最小二乗推定 [J]. Li Zhongmei. Journal of Surveying and Mapping