作者：李城
點擊上方“3D視覺工坊”，選擇“星標”
干貨第一時間送達
提到三角化大家都十分熟悉，在CV 領域中，由像點計算物點的過程稱為三角化，但在攝影測量領域，其稱作為前方交會。值得注意的是單張影像是無法恢復像點的三維坐標，至少需要兩張影像才能得到像素點的真實坐標(這里已知兩張影像的pose信息)
三角化有很多方法，這里介紹兩幀三角化、多幀三角化、迭代三角化、選權迭代多幀三角化（並附上本人代碼）。
1、兩幀三角化
在opencv 中函數triangulatePoints就可根據兩幀的pose 和內參恢復三維點坐標，cv中的三角化是兩幀且是沒有權的。
其函數參數如下：
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、多幀三角化
三角化嚴密解推導過程：
由共線條件方程得到三角化嚴密解法，將分母移到左邊，得到
整理可得：
l1X+l3Y+l3Z−lx=0L4X+l5Y+l6Z−ly=0
其中：
從上可以知道，一個像點可以列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算法)，上世紀 80 年代，首創從驗后方差估計導出粗差定位的選權迭代法，命名為“李德仁方法”。在實際測量工作中客觀條件的限制,很難完全避免粗差的存在或做到完全同等精度量測.在平差過程中,通常引入權作為比較觀測值之間相對精度高低的指標,並為精度較高的觀測數據賦予較高的權重，這樣就可規避有害信息的干擾。例如，我們在image matching 的匹配的時候，會用到ransac(同樣是穩健估計算法) 剔除outlier，但是當你的同名點是在多幀上且只有一個的時候（比如多幀紅綠燈的位置測量），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].李忠美.測繪學報
本文僅做學術分享，如有侵權，請聯系刪文。
下載1
在「3D視覺工坊」公眾號后台回復：3D視覺，即可下載 3D視覺相關資料干貨，涉及相機標定、三維重建、立體視覺、SLAM、深度學習、點雲后處理、多視圖幾何等方向。
下載2
在「3D視覺工坊」公眾號后台回復：3D視覺github資源匯總，即可下載包括結構光、標定源碼、缺陷檢測源碼、深度估計與深度補全源碼、點雲處理相關源碼、立體匹配源碼、單目、雙目3D檢測、基於點雲的3D檢測、6D姿態估計源碼匯總等。
下載3
在「3D視覺工坊」公眾號后台回復：相機標定，即可下載獨家相機標定學習課件與視頻網址；后台回復：立體匹配，即可下載獨家立體匹配學習課件與視頻網址。
重磅！3DCVer-學術論文寫作投稿 交流群已成立掃碼添加小助手微信，可申請加入3D視覺工坊-學術論文寫作與投稿 微信交流群，旨在交流頂會、頂刊、SCI、EI等寫作與投稿事宜。
同時也可申請加入我們的細分方向交流群，目前主要有3D視覺、CV&深度學習、SLAM、三維重建、點雲后處理、自動駕駛、CV入門、三維測量、VR/AR、3D人臉識別、醫療影像、缺陷檢測、行人重識別、目標跟蹤、視覺產品落地、視覺競賽、車牌識別、硬件選型、學術交流、求職交流、ORB-SLAM系列源碼交流、深度估計等微信群。一定要備注：研究方向+學校/公司+昵稱，例如：”3D視覺 + 上海交大 + 靜靜“。請按照格式備注，可快速被通過且邀請進群。原創投稿也請聯系。▲長按加微信群或投稿▲長按關注公眾號
3D視覺從入門到精通知識星球：針對3D視覺領域的知識點匯總、入門進階學習路線、最新paper分享、疑問解答四個方面進行深耕，更有各類大廠的算法工程人員進行技術指導。與此同時，星球將聯合知名企業發布3D視覺相關算法開發崗位以及項目對接信息，打造成集技術與就業為一體的鐵桿粉絲聚集區，近2000星球成員為創造更好的AI世界共同進步，知識星球入口：
學習3D視覺核心技術，掃描查看介紹，3天內無條件退款 圈里有高質量教程資料、可答疑解惑、助你高效解決問題覺得有用，麻煩給個贊和在看~