Error message here!

Hide Error message here!

忘记密码?

Error message here!

请输入正确邮箱

Hide Error message here!

密码丢失?请输入您的电子邮件地址。您将收到一个重设密码链接。

Error message here!

返回登录

Close

摄影测量(计算机视觉)中的三角化方法

3D视觉工坊 2020-11-19 17:03:31 阅读数:99 评论数:0 点赞数:0 收藏数:0

提到三角化大家都十分熟悉,在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].李忠美.测绘学报

本文仅做学术分享,如有侵权,请联系删文。

本文分享自微信公众号 - 3D视觉工坊(QYong_2014)

原文出处及转载信息见文内详细说明,如有侵权,请联系 yunjia_community@tencent.com 删除。

原始发表时间: 2020-11-12

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

版权声明
本文为[3D视觉工坊]所创,转载请带上原文链接,感谢
https://cloud.tencent.com/developer/article/1749499

支付宝红包,每日可领(支付宝免费1-2元支付红包)