摄影测量与计算机视觉（摄影测量计算机视觉）
摄影测量与计算机视觉（摄影测量计算机视觉）其函数参数如下：在opencv 中函数triangulatePoints就可根据两帧的pose 和内参恢复三维点坐标，cv中的三角化是两帧且是没有权的。提到三角化大家都十分熟悉，在CV 领域中，由像点计算物点的过程称为三角化，但在摄影测量领域，其称作为前方交会。值得注意的是单张影像是无法恢复像点的三维坐标，至少需要两张影像才能得到像素点的真实坐标(这里已知两张影像的pose信息)三角化有很多方法，这里介绍两帧三角化、多帧三角化、迭代三角化、选权迭代多帧三角化（并附上本人代码）。 1、两帧三角化
作者：李城
来源：微信公众号|3D视觉工坊（系投稿）
「3D视觉工坊」技术交流群已经成立，目前大约有12000人，方向主要涉及3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等。工坊致力于干货输出，不做搬运工，为计算机视觉领域贡献自己的力量！欢迎大家一起交流成长~
添加小助手CV_LAB，备注学校/公司 姓名 研究方向即可加入工坊一起学习进步。
提到三角化大家都十分熟悉，在CV 领域中，由像点计算物点的过程称为三角化，但在摄影测量领域，其称作为前方交会。值得注意的是单张影像是无法恢复像点的三维坐标，至少需要两张影像才能得到像素点的真实坐标(这里已知两张影像的pose信息)
三角化有很多方法，这里介绍两帧三角化、多帧三角化、迭代三角化、选权迭代多帧三角化（并附上本人代码）。
1、两帧三角化
在opencv 中函数triangulatePoints就可根据两帧的pose 和内参恢复三维点坐标，cv中的三角化是两帧且是没有权的。
其函数参数如下：
voidcv::triangulatePoints(InputArrayprojMatr1 //P113*4
InputArrayprojMatr2 //P23*4
InputArrayprojPoints1 //pixelcoordinates
InputArrayprojPoints2 //pixelcoordinates
OutputArraypoints4D//3Dcoordinates
)
2、多帧三角化
三角化严密解推导过程：
由共线条件方程得到三角化严密解法，将分母移到左边，得到
整理可得：
l1X l3Y l3Z−lx=0L4X l5Y l6Z−ly=0
其中：
从上可以知道，一个像点可以列2个方程，对于n个同名像点就可以列2n个方程(AX=B，超定方程按照最小二乘求解)，即是多帧三角化 代码如下：
defpixelToCam(pts K):
'''
:parampts:pixelcoordinates
:paramK:cameraparams
:return:cameracoordinates
'''
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]
returncamPts
defgetEquation(camPts R t):
'''
buildequation onepixelpointget2equations
:paramcamPts:cameracoordinates
:paramR:imagepose-rotation worldtocamera
:paramt:imagepose-translation iscameracenter(t=-R.T*tvec)
:return:equationcoefficient
'''
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]
returnA b
3 、迭代三角化
其做法就是在方程系数加入因子，不断调整系数的因子，本人代码如下：
defIterativeLinearLSTri(u1 P1 u2 P2):
wi wi1=1 1#不断需要更新的因子
X=np.zeros((4 1))
foriinrange(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]
ifabs(wi-p2x)<=0.001andabs(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
returnX
4、选权迭代多帧三角化
首先解释选权迭代（IGG算法)，上世纪 80 年代，首创从验后方差估计导出粗差定位的选权迭代法，命名为“李德仁方法”。在实际测量工作中客观条件的限制 很难完全避免粗差的存在或做到完全同等精度量测.在平差过程中 通常引入权作为比较观测值之间相对精度高低的指标 并为精度较高的观测数据赋予较高的权重，这样就可规避有害信息的干扰。例如，我们在image matching 的匹配的时候，会用到ransac(同样是稳健估计算法) 剔除outlier，但是当你的同名点是在多帧上且只有一个的时候（比如多帧红绿灯的位置测量），ransac 就不能再使用，这个时候使用IGG 算法可以有效的规避误点带来影响，论文参考-多像空间前方交会的抗差总体最小二乘估计，本人将论文的算法进行了复现，代码如下：
defIterationInsection(pts K R t):
#cam_xyzisinitalvalue
#这里假设像点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
while1:
d=np.zeros((len(pts) 1))
foriinrange(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))
foriinrange(len(pts)):
u=d[i]
ifabs(u)<k0*delta:
w[i]=1
elifabs(u)<k1*deltaandabs(u)>=k0*delta:
w[i]=delta/u
elifabs(u)>=k1*delta:
w[i]=0
weight_temp=w
weight_p=[valforvalinweight_temp.reshape(-1 )foriinrange(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)
ifnp.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("iterationis{0}\n".format(iteration))
print("IGG....{0} {1} {2}".format(cam_xyz[0 0] cam_xyz[1 0] cam_xyz[2 0]))
returncam_xyz weight
其中mutiTriangle 函数和weight_mutiTriangle函数如下：
defmutiTriangle(pts K R t):
iflen(pts)>=4:#这里是假设至少track4帧
equa_A=[]
equa_b=[]
foriinrange(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)
returnP_ls
else:
print("trackerpixelpointless3 cannotinsection........")
returnNone
defweight_mutiTriangle(pts K R t weight):
iflen(pts)>=4:
equa_A=[]
equa_b=[]
foriinrange(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)
returnP_ls
else:
print("trackerpixelpointless4 cannotinsection........")
returnNone
参考文献：
1、多像空间前方交会的抗差总体最小二乘估计[J].李忠美.测绘学报
本文仅做学术分享，如有侵权，请联系删文。
