【python计算机视觉编程——多视图几何】

5.多视图几何

5.1 外极几何

该网站可以下载图像点、三维点和照相机参数矩阵的数据集Visual Geometry Group - University of Oxford

from PIL import Image
matplotlib.rcParams['font.family'] = 'sans-serif'
matplotlib.rcParams['font.sans-serif'] = ['SimHei']  # 黑体字体
matplotlib.rcParams['axes.unicode_minus'] =False  # 显示负号

class Camera(object):def __init__(self,P):self.P=P      #照相机矩阵self.K=None   #标定矩阵self.R=None   #旋转矩阵self.t=None   #平移向量self.c=None   #照相机中心def project(self,X):x=np.dot(self.P,X)  #得到一个包含齐次坐标的投影结果 xfor i in range(3):x[i]/=x[2]  # 以实现归一化。return xdef factor(self):K,R=linalg.rq(self.P[:,:3]) # 将照相机矩阵 P 的左上 3x3 部分分解为上三角矩阵 K 和正交矩阵 RT=diag(sign(diag(K)))  # 构造对角矩阵 T，其对角线元素为 K 对角线元素的符号。if linalg.det(T)<0:  # 检查 T 的行列式是否为负。如果是，则调整 T 使得行列式为正。T[1,1]*=-1self.K=dot(K,T)self.R=dot(T,R)self.t=dot(linalg.inv(self.K),self.P[:,3])  # linalg.inv(self.K)用于计算标定矩阵 K 的逆矩阵return self.K,self.R,self.tdef center(self):if self.c is not None:return self.celse:self.factor()self.c=-dot(self.R.T,self.t)  # 将平移向量转化为相机坐标系原点在世界坐标系中的位置return self.c

im1=array(Image.open('images/001.jpg'))
im2=array(Image.open('images/002.jpg'))points2D=[loadtxt('2D/00'+str(i+1)+'.corners').T for i in range(3)]points3D=loadtxt('3D/p3d').Tcorr=genfromtxt('2D/nview-corners',dtype='int')P=[Camera(loadtxt('2D/00'+str(i+1)+'.P')) for i in range(3)]

X = vstack( (points3D, ones(points3D.shape[1])))
x = P[0].project(X)# 在视图1中绘制点
figure(figsize=(10, 10))
subplot(121)
imshow(im1)
plot(points2D[0][0], points2D[0][1],'*')
axis('off')
title('视图 1 与图像点')
subplot(122)
imshow(im1),plot(x[0],x[1],'r.')
axis('off')
title('视图 1 与投影的三维点')
# savefig('./output/test1.jpg')
show()

5.1.2 用Matplotlib绘制三维数据

from mpl_toolkits.mplot3d import Axes3D
from pylab import *
import numpy as np

%matplotlib notebook
fig=figure()
# ax=fig.gca(projection="3d")
ax = fig.add_axes(Axes3D(fig))X,Y,Z=axes3d.get_test_data(0.25)
ax.plot(X.flatten(),Y.flatten(),Z.flatten(),'o')ax.set_xlabel('X轴')
ax.set_ylabel('Y轴')
ax.set_zlabel('Z轴')
show()

%matplotlib notebook
fig=figure()
ax = fig.add_axes(Axes3D(fig))
ax.plot(points3D[0],points3D[1],points3D[2],'k.')
ax.set_xlabel('X轴')
ax.set_ylabel('Y轴')
ax.set_zlabel('Z轴')

5.1.3 计算F：八点法

def compute_fundamental(x1, x2):"""使用归一化的八点算法，从对应点（x1，x2 3×n的数组）中计算基础矩阵每行由如下构成：[x'*x, x'*y' x', y'*x, y'*y, y', x, y, 1]"""n = x1.shape[1]if x2.shape[1] != n:raise ValueError("Number of points don't match.")# 创建方程对应的矩阵A = zeros((n,9))for i in range(n):A[i] = [x1[0, i] * x2[0, i], x1[0, i] * x2[1, i], x1[0, i] * x2[2, i],x1[1, i] * x2[0, i], x1[1, i] * x2[1, i], x1[1, i] * x2[2, i],x1[2, i] * x2[0, i], x1[2, i] * x2[1, i], x1[2, i] * x2[2, i]]# 计算线性最小二乘解U,S,V = linalg.svd(A)F = V[-1].reshape(3,3)# 受限F# 通过将最后一个奇异值置0，使秩为2U,S,V = linalg.svd(F)S[2] = 0F = dot(U, dot(diag(S), V))return F

5.1.4 外极点和外极线

def compute_epipole(F):"""从基础矩阵F中计算右极点（可以使用F.T获得左极点）"""# 返回F的零空间（Fx=0）U,S,V = linalg.svd(F)e = V[-1]return e/e[2]

def plot_epipolar_line(im,F,x,epipole=None,show_epipole=True):m,n=im.shape[:2]line=dot(F,x)t=linspace(0,n,100)lt=array([(line[2]+line[0]*tt)/(-line[1]) for tt in t])ndx=(lt>=0)&(lt<m)plot(t[ndx],lt[ndx],linewidth=2)if show_epipole:if epipole is None:epipole=compute_epipole(F)plot(epipole[0]/epipole[2],epipole[1]/epipole[2],'r*')

%matplotlib notebook
ndx = (corr[:,0]>=0) & (corr[:,1]>=0)#获得坐标，并将其用齐次坐标表示
x1 = points2D[0][:,corr[ndx,0]]
x1 = vstack((x1,ones(x1.shape[1])))
x2 = points2D[1][:,corr[ndx,1]]
x2 = vstack((x2,ones(x2.shape[1])))#计算F
F = compute_fundamental(x1,x2)#计算极点
e = compute_epipole(F)#绘制图像
figure()
imshow(im1)#分别绘制每条线，这样会绘制出很漂亮的颜色
for i in range(5):plot_epipolar_line(im1,F,x2[:,i],e)
axis('off')
savefig('plot_epipolar_line.jpg')figure()
imshow(im2)#分别绘制每个点，这样绘制出和线同样的颜色
for i in range(5):plot(x2[0,i],x2[1,i],'o')
axis('off')
show()

5.2 照相机和三维结构的计算

5.2.1 三角部分

from mpl_toolkits.mplot3d import Axes3D
from pylab import *
from PIL import Image
matplotlib.rcParams['font.family'] = 'sans-serif'
matplotlib.rcParams['font.sans-serif'] = ['SimHei']  # 黑体字体
matplotlib.rcParams['axes.unicode_minus'] =False  # 显示负号
import os

def triangulate_point(x1,x2,P1,P2):M=zeros((6,6))M[:3,:4]=P1M[3:,:4]=P2M[:3,4]=-x1M[3:,5]=-x2U,S,V=linalg.svd(M)X=V[-1,:4]return X/X[3]

def triangulate(x1,x2,P1,P2):n=x1.shape[1]if x2.shape[1]!=n:raise ValueError("Number of points don`t match.")X=[triangulate_point(x1[:,i],x2[:,i],P1,P2) for i in range(n)]return array(X).T

这里还需要用到之前的Camera类

class Camera(object):def __init__(self,P):self.P=P      #照相机矩阵self.K=None   #标定矩阵self.R=None   #旋转矩阵self.t=None   #平移向量self.c=None   #照相机中心def project(self,X):x=np.dot(self.P,X)  #得到一个包含齐次坐标的投影结果 xfor i in range(3):x[i]/=x[2]  # 以实现归一化。return xdef factor(self):K,R=linalg.rq(self.P[:,:3]) # 将照相机矩阵 P 的左上 3x3 部分分解为上三角矩阵 K 和正交矩阵 RT=diag(sign(diag(K)))  # 构造对角矩阵 T，其对角线元素为 K 对角线元素的符号。if linalg.det(T)<0:  # 检查 T 的行列式是否为负。如果是，则调整 T 使得行列式为正。T[1,1]*=-1self.K=dot(K,T)self.R=dot(T,R)self.t=dot(linalg.inv(self.K),self.P[:,3])  # linalg.inv(self.K)用于计算标定矩阵 K 的逆矩阵return self.K,self.R,self.tdef center(self):if self.c is not None:return self.celse:self.factor()self.c=-dot(self.R.T,self.t)  # 将平移向量转化为相机坐标系原点在世界坐标系中的位置return self.c

%matplotlib notebookndx = (corr[:,0]>=0) & (corr[:,1]>=0)#获得坐标，并将其用齐次坐标表示
x1 = points2D[0][:,corr[ndx,0]]
x1 = vstack((x1,ones(x1.shape[1])))
x2 = points2D[1][:,corr[ndx,1]]
x2 = vstack((x2,ones(x2.shape[1])))Xtrue=points3D[:,ndx]
Xtrue=vstack((Xtrue,ones(Xtrue.shape[1])))Xest=triangulate(x1,x2,P[0].P,P[1].P)
print(Xest[:,:3])
print(Xtrue[:,:3])fig=figure()
ax = fig.add_axes(Axes3D(fig))
ax.plot(Xest[0],Xest[1],Xest[2],'ko')
ax.plot(Xtrue[0],Xtrue[1],Xtrue[2],'r.')
axis('equal')show()

5.2.2 由三维点计算照相机矩阵

def compute_P(x,X):n=x.shape[1]if X.shape[1]!=n:raise ValueError("Number of points don`t match.")M=zeros((3*n,12+n))for i in range(n):M[3*i,0:4]=X[:,i]M[3*i+1,4:8]=X[:,i]M[3*i+2,8:12]=X[:,i]M[3*i:3*i+3,i+12]=x[:,i]U,S,V=linalg.svd(M)return V[-1,:12].reshape((3,4))

corr=corr[:,0]
ndx3D=where(corr>=0)[0]
ndx2D=corr[ndx3D]x=points2D[0][:,ndx2D]
x=vstack((x,ones(x.shape[1])))
X=points3D[:,ndx3D]
X=vstack((X,ones(X.shape[1])))Pest=Camera(compute_P(x,X))print(Pest.P/Pest.P[2,3])
print(P[0].P/P[0].P[2,3])xest=Pest.project(X)figure()
imshow(im1)
plot(x[0],x[1],'bo')
plot(xest[0],xest[1],'r.')
axis('off')show()

5.2.3 由基础矩阵计算照相机矩阵

def compute_P_from_fundamental(F):e=compute_epipole(F.T)Te=skew(e)return vstack((dot(Te,F.T).T,e)).T

def skew(a):return array([[0,-a[2],a[1]],[a[2],0,-a[0]],[-a[1],a[0],0]])

def compute_P_from_essential(E):U,S,V=svd(E)if det(dot(U,V))<0:V=-VE=dot(U,dot(diag([1,1,0]),V))Z=skew([0,0,-1])W=array([[0,-1,0],[1,0,0],[0,0,1]])P2=[vstack((dot(U,dot(W,V)).T,U[:,2])).T,vstack((dot(U,dot(W,V)).T,-U[:,2])).T,vstack((dot(U,dot(W.T,V)).T,U[:,2])).T,vstack((dot(U,dot(W.T,V)).T,-U[:,2])).T]return P2

5.3 多视图重建

5.3.1 稳健估计基础矩阵

class RansacModel(object):def __init__(self,debug=False):self.debug=debugdef fit(self,data):data=data.Tx1=data[:3,:8]x2=data[3:,:8]F=compute_fundamental_normalized(x1,x2)return Fdef get_error(self,data,F):data=data.Tx1=data[:3]x2=data[3:]Fx1=dot(F,x1)Fx2=dot(F,x2)denom=Fx1[0]**2+Fx1[1]**2+Fx2[0]**2+Fx2[1]**2err=(diag(dot(x1.T,dot(F,x2))))**2/denomreturn err

 def compute_fundamental_normalized(x1, x2):n = x1.shape[1]if x2.shape[1] != n:raise ValueError("Number of points don't match.")# normalize image coordinatesx1 = x1 / x1[2]mean_1 = np.mean(x1[:2], axis=1)S1 = np.sqrt(2) / np.std(x1[:2])T1 = np.array([[S1, 0, -S1 * mean_1[0]], [0, S1, -S1 * mean_1[1]], [0, 0, 1]])x1 = np.dot(T1, x1)x2 = x2 / x2[2]mean_2 = np.mean(x2[:2], axis=1)S2 = np.sqrt(2) / np.std(x2[:2])T2 = np.array([[S2, 0, -S2 * mean_2[0]], [0, S2, -S2 * mean_2[1]], [0, 0, 1]])x2 = np.dot(T2, x2)# compute F with the normalized coordinatesF = compute_fundamental(x1, x2)# print (F)# reverse normalizationF = np.dot(T1.T, np.dot(F, T2))return F / F[2, 2]

def random_partition(n,n_data):"""return n random rows of data (and also the other len(data)-n rows)"""all_idxs = np.arange( n_data )np.random.shuffle(all_idxs)idxs1 = all_idxs[:n]idxs2 = all_idxs[n:]return idxs1, idxs2

def ransac(data, model, n, k, t, d, debug=False, return_all=False):iterations = 0bestfit = Nonebesterr = np.infbest_inlier_idxs = Nonewhile iterations < k:maybe_idxs, test_idxs = random_partition(n, data.shape[0])maybeinliers = data[maybe_idxs, :]test_points = data[test_idxs]maybemodel = model.fit(maybeinliers)test_err = model.get_error(test_points, maybemodel)also_idxs = test_idxs[test_err < t]  # select indices of rows with accepted pointsalsoinliers = data[also_idxs, :]if debug:print('test_err.min()', test_err.min())print('test_err.max()', test_err.max())print('numpy.mean(test_err)', np.mean(test_err))print('iteration %d:len(alsoinliers) = %d' %(iterations, len(alsoinliers)))if len(alsoinliers) > d:betterdata = np.concatenate((maybeinliers, alsoinliers))bettermodel = model.fit(betterdata)better_errs = model.get_error(betterdata, bettermodel)#重新计算总的errorthiserr = np.mean(better_errs)if thiserr < besterr:bestfit = bettermodelbesterr = thiserrbest_inlier_idxs = np.concatenate((maybe_idxs, also_idxs))iterations += 1if bestfit is None:raise ValueError("did not meet fit acceptance criteria")if return_all:return bestfit, {'inliers': best_inlier_idxs}else:return bestfit

def F_from_ransac(x1,x2,model,maxiter=5000,match_theshold=1e-4):
# def F_from_ransac(x1,x2,model,maxiter=5000,match_theshold=1e-6):data = vstack((x1,x2))F,ransac_data = ransac(data.T,model,8,maxiter,match_theshold,20,return_all=True)return F, ransac_data['inliers']

5.3.2 三维重建示例

• sift文件

def process_image(imagename, resultname, params="--edge-thresh 10 --peak-thresh 5"):if imagename[-3:] != 'pgm':im = Image.open(imagename).convert('L')im.save('tmp.pgm')imagename = 'tmp.pgm'cmmd = str(".\sift.exe " + imagename + " --output=" + resultname + " " + params)os.system(cmmd)print('processed', imagename, 'to', resultname)

def read_features_from_file(filename):f=loadtxt(filename)return f[:,:4],f[:,4:]

def match(desc1,desc2):desc1=array([d/linalg.norm(d) for d in desc1])desc2=array([d/linalg.norm(d) for d in desc2])dist_ratio=0.6desc1_size=desc1.shapematchscores=zeros((desc1_size[0],1),'int')desc2t=desc2.Tfor i in range(desc1_size[0]):dotprods=dot(desc1[i,:],desc2t)dotprods=0.9999*dotprodsindx=argsort(arccos(dotprods))if arccos(dotprods)[indx[0]]<dist_ratio*arccos(dotprods)[indx[1]]:matchscores[i]=int(indx[0])return matchscores

def match_twosided(desc1,desc2):matches_12=match(desc1,desc2)
#     print(matches_12.shape)matches_21=match(desc2,desc1)ndx_12=matches_12.nonzero()[0] #获取的是与desc2匹配的desc1的索引值for n in ndx_12:if matches_21[int(matches_12[n])]!=n:  # matches_12[n]是desc1中第n个描述符在desc2中的匹配索引matches_12[n]=0return matches_12

• homography文件

def make_homog(points): return np.vstack((points,np.ones((1,points.shape[1]))))

def H_from_points(fp,tp):if fp.shape!=tp.shape: # 确保源点 (fp) 和目标点 (tp) 的形状相同。如果形状不匹配，抛出异常。raise RuntimeError('number of points do not match')# 归一化源点:计算源点的均值m和标准差maxstd。创建归一化矩阵C1，用于将源点fp进行归一化处理，以减小计算中的数值误差。m=np.mean(fp[:2],axis=1)  # 计算源点的均值 m，对每个坐标分量进行均值计算maxstd=max(np.std(fp[:2],axis=1))+1e-9    # 计算源点的标准差 maxstd，加一个小偏移量以避免除零错误  C1=np.diag([1/maxstd,1/maxstd,1])   # 创建归一化矩阵 C1，用于缩放坐标C1[0][2]=-m[0]/maxstd  # 设置 C1 矩阵的平移部分C1[1][2]=-m[1]/maxstd  # 设置 C1 矩阵的平移部分fp=np.dot(C1,fp)       # 应用归一化矩阵 C1 到源点 fp# 归一化目标点:类似地，对目标点进行归一化处理，计算均值和标准差，并应用归一化矩阵 C2。m=np.mean(tp[:2],axis=1)maxstd=max(np.std(tp[:2],axis=1))+1e-9C2=np.diag([1/maxstd,1/maxstd,1])C2[0][2]=-m[0]/maxstdC2[1][2]=-m[1]/maxstdtp=np.dot(C2,tp)nbr_correspondences=fp.shape[1]A=np.zeros((2*nbr_correspondences,9)) #构建矩阵A，用于求解单应性矩阵。每对点提供两个方程，总共 2 * nbr_correspondences 行。for i in range(nbr_correspondences):A[2*i]=[-fp[0][i],-fp[1][i],-1,0,0,0,tp[0][i]*fp[0][i],tp[0][i]*fp[1][i],tp[0][i]]A[2*i+1]=[0,0,0,-fp[0][i],-fp[1][i],-1,tp[1][i]*fp[0][i],tp[1][i]*fp[1][i],tp[1][i]]U,S,V=np.linalg.svd(A) # 使用奇异值分解 (SVD)求解矩阵A的最小特征值对应的向量。取V的最后一行（对应于最小特征值），重塑为3x3矩阵 HH=V[8].reshape((3,3))
#     反归一化H=np.dot(np.linalg.inv(C2),np.dot(H,C1))# 使用逆归一化矩阵将计算得到的单应性矩阵从归一化坐标系转换回原始坐标系，并进行归一化处理。
#     归一化，然后返回return H/H[2,2]

class RansacModel(object):""" 用于测试单应性矩阵的类，其中单应性矩阵是由网站 http://www.scipy.org/Cookbook/RANSAC 上的 ransac.py 计算出来的"""def __init__(self,debug=False):self.debug=debug def fit(self,data):""" 计算选取的 4 个对应的单应性矩阵 """# 将其转置，来调用 H_from_points() 计算单应性矩阵data = data.T# 映射的起始点fp = data[:3,:4]# 映射的目标点tp = data[3:,:4]# 计算单应性矩阵，然后返回return H_from_points(fp,tp)def get_error(self, data, H):""" 对所有的对应计算单应性矩阵，然后对每个变换后的点，返回相应的误差 """data = data.T# 映射的起始点fp = data[:3]# 映射的目标点tp = data[3:]# 变换fpfp_transformed = dot(H,fp)# 归一化齐次坐标for i in range(3):fp_transformed[i]/=fp_transformed[2]# 返回每个点的误差return sqrt( sum((tp-fp_transformed)**2,axis=0) )

K=array([[2394,0,932],[0,2398,628],[0,0,1]])
im1=array(Image.open('alcatraz1.jpg'))
process_image('alcatraz1.jpg','im1.sift')
l1,d1=read_features_from_file('im1.sift')im2=array(Image.open('alcatraz2.jpg'))
process_image('alcatraz2.jpg','im2.sift')
l2,d2=read_features_from_file('im2.sift')matches=match_twosided(d1,d2)
ndx=matches.nonzero()[0]x1=make_homog(l1[ndx,:2].T)
ndx2=[int(matches[i]) for i in ndx]
x2=make_homog(l2[ndx2,:2].T)

x1n=dot(inv(K),x1)
x2n=dot(inv(K),x2)
model=RansacModel()
E,inliers=F_from_ransac(x1n,x2n,model)
P1=array([[1,0,0,0],[0,1,0,0],[0,0,1,0]])
P2=compute_P_from_essential(E)

ind=0
maxres=0
for i in range(4):X=triangulate(x1n[:,inliers],x2n[:,inliers],P1,P2[i])d1=dot(P1,X)[2]d2=dot(P2[i],X)[2]if sum(d1>0)+sum(d2>0)>maxres:maxres=sum(d1>0)+sum(d2>0)ind=iinfront=(d1>0)&(d2>0)X=triangulate(x1n[:,inliers],x2n[:,inliers],P1,P2[ind])X=X[:,infront]

# 绘制X的投影 import camera
# 绘制三维点
cam1 = Camera(P1)
cam2 = Camera(P2[ind])
x1p = cam1.project(X)
x2p = cam2.project(X)# 反K归一化
x1p = dot(K, x1p)
x2p = dot(K, x2p)
figure()
imshow(im1)
gray()
plot(x1p[0], x1p[1], 'o')
plot(x1[0], x1[1], 'r.')
axis('off')
figure()
imshow(im2)
gray()
plot(x2p[0], x2p[1], 'o')
plot(x2[0], x2[1], 'r.')
axis('off')
show()

5.4 立体图像

from scipy.ndimage import filters
def plane_sweep_ncc(im_l,im_r,start,steps,wid):m,n=im_l.shapemean_l=zeros((m,n))mean_r=zeros((m,n))s=zeros((m,n))s_l=zeros((m,n))s_r=zeros((m,n))dmaps=zeros((m,n,steps))filters.uniform_filter(im_l,wid,mean_l)filters.uniform_filter(im_r,wid,mean_r)norm_l=im_l-mean_lnorm_r=im_r-mean_rfor displ in range(steps):filters.uniform_filter(roll(norm_l,-displ-start)*norm_r,wid,s)filters.uniform_filter(roll(norm_l,-displ-start)*roll(norm_l,-displ-start),wid,s_l)filters.uniform_filter(norm_r*norm_r,wid,s_r)dmaps[:,:,displ]=s/sqrt(s_l*s_r)return argmax(dmaps,axis=2)

def plane_sweep_gauss(im_l,im_r,start,steps,wid):m,n = im_l.shape# arrays to hold the different sumsmean_l = zeros((m,n))mean_r = zeros((m,n))s = zeros((m,n))s_l = zeros((m,n))s_r = zeros((m,n))# array to hold depth planesdmaps = zeros((m,n,steps))# compute meanfilters.gaussian_filter(im_l,wid,0,mean_l)filters.gaussian_filter(im_r,wid,0,mean_r)# normalized imagesnorm_l = im_l - mean_lnorm_r = im_r - mean_r# try different disparitiesfor displ in range(steps):# move left image to the right, compute sumsfilters.gaussian_filter(roll(norm_l,-displ-start)*norm_r,wid,0,s) # sum nominatorfilters.gaussian_filter(roll(norm_l,-displ-start)*roll(norm_l,-displ-start),wid,0,s_l)filters.gaussian_filter(norm_r*norm_r,wid,0,s_r) # sum denominator# store ncc scoresdmaps[:,:,displ] = s/sqrt(s_l*s_r)# pick best depth for each pixelreturn argmax(dmaps,axis=2)

ppm文件可从该网站获取

Index of /stereo/data/scenes2001/data/tsukuba (middlebury.edu)

im_l=array(Image.open('scene1.row3.col3.ppm').convert('L'),'f')
im_r=array(Image.open('scene1.row3.col4.ppm').convert('L'),'f')steps=12
start=4wid=9res_ncc=plane_sweep_ncc(im_l,im_r,start,steps,wid)
import cv2
cv2.imwrite('depth_ncc.png',res_ncc)

im_l=array(Image.open('scene1.row3.col3.ppm').convert('L'),'f')
im_r=array(Image.open('scene1.row3.col4.ppm').convert('L'),'f')steps=12  # 视差步数
start=4
wid=9    # 窗口宽度res_gauss=plane_sweep_gauss(im_l,im_r,start,steps,wid)import cv2
cv2.imwrite('depth_gauss.png',res_gauss)

# 显示视差图
subplot(121)
imshow(im_l)subplot(122)
imshow(res_ncc, cmap='jet')
colorbar()show()