五、【python计算机视觉编程】多视图几何
多视图几何
- （一）外极几何
- （1）一个简单的数据集
- （2）用matplotlib绘制三维数据
- （3）计算F：八点法
- （4）外极点和外极线
- （二）照相机和三维结构的计算
- （1）三角剖分
- （2）由三维点计算照相机矩阵
- （3）由基础矩阵计算照相机矩阵
- （三）多视图重建
- （1）稳健估计基础矩阵
- （2）三维重建示例
- （3）多视图的扩展示例
- （四）立体图像
（一）外极几何
多视图几何时利用在不同视点所拍摄图像间的关系，来研究照相机之间或者特征之间关系的一门科学。多视图几何中最重要的内容是双视图几何。如果有一个场景的两个视图以及视图中的对应图像点，那么根据照相机的空间相对位置关系、照相机的性质以及三维场景点的位置，可以得到对这些图像点的一些几何关系约束。通过外极几何来描述这些几何关系。 其示意图如下所示：
在这两个视图中，分别将三维点X投影为x1和x2.两个相机中心之间的基线C1和C2与图像平面相交于外极点e1和e2，线I1和I2成为外极线。如果已经知道相机的参数，那么在重建过程中遇到的问题就是两幅图像之间的关系，外极线约束的主要作用就是限制对应特征点的搜索范围，将对应特征点的搜索限制在极线上。
基础矩阵：
1.概述：
1）两幅图像之间的约束关系使用代数的方式表示出来即为基本矩阵。
2）基础矩阵F满足： x T F x ′ = 0 x^{T}F{x}'=0 xTFx′=0
3）基础矩阵可以用于简化匹配和去除错配特征。
2.原理：
设X在 C C C, C ′ C' C′坐标系中的相对坐标分别为 p p p, p ′ p' p′，则有 ： p = R p ′ + T p=Rp'+T p=Rp′+T 其中， x = K ′ p x=K'p x=K′p x ′ = K p ′ x'=Kp' x′=Kp′ p = K − 1 x p=K^{-1}x p=K−1x p ′ = K ′ − 1 x ′ p'=K'^{-1}x' p′=K′−1x′
根据三线共面，有： ( p − T ) T ( T × p ) = 0 (p-T)^{T}(T×p)=0 (p−T)T(T×p)=0 转换为 ( R T p ′ ) T ( T × p ) = 0 (R^{T}p')^{T}(T×p)=0 (RTp′)T(T×p)=0 ⇨ T × p = S p T×p=Sp T×p=Sp
本质矩阵描述的是：空间中的点在两 个坐标系中的坐标对应关系。
根据前述， K K K和 K ′ K' K′分别为两个相机的内参矩阵，有：
基础矩阵描述的是： 空间中的点在两个相平面中的坐标对应关系。
3.应用：
（1）简化匹配
（2）去除错配特征
根据基本矩阵编写代码：
# -*- coding: utf-8 -*-
from PIL import Image
from numpy import *
from pylab import *
import numpy as np
from PCV.geometry import camera
from PCV.geometry import homography
from PCV.geometry import sfm
from PCV.localdescriptors import sift
from importlib import reload
camera = reload(camera)
homography = reload(homography)
sfm = reload(sfm)
sift = reload(sift)
# Read features
im1 = array(Image.open('picture/home/1.jpg'))
im2 = array(Image.open('picture/home/2.jpg'))
sift.process_image('picture/home/1.jpg', 'im1.sift')
l1, d1 =sift.read_features_from_file('im0.sift')
sift.process_image('picture/home/2.jpg', 'im2.sift')
l2, d2 =sift.read_features_from_file('im1.sift')
matches = sift.match_twosided(d1, d2)
ndx = matches.nonzero()[0]
x1 = homography.make_homog(l1[ndx, :2].T)
ndx2 = [int(matches[i]) for i in ndx]
x2 = homography.make_homog(l2[ndx2, :2].T)
d1n = d1[ndx]
d2n = d2[ndx2]
x1n = x1.copy()
x2n = x2.copy()
figure(figsize=(16,16))
sift.plot_matches(im1, im2, l1, l2, matches, True)
show()
#def F_from_ransac(x1, x2, model, maxiter=5000, match_threshold=1e-6):
def F_from_ransac(x1, x2, model, maxiter=5000, match_threshold=1e-6):
""" Robust estimation of a fundamental matrix F from point
correspondences using RANSAC (ransac.py from
http://www.scipy.org/Cookbook/RANSAC).
input: x1, x2 (3*n arrays) points in hom. coordinates. """
from PCV.tools import ransac
data = np.vstack((x1, x2))
d = 10 # 20 is the original
# compute F and return with inlier index
F, ransac_data = ransac.ransac(data.T, model,
8, maxiter, match_threshold, d, return_all=True)
return F, ransac_data['inliers']
# find F through RANSAC
model = sfm.RansacModel()
F, inliers = F_from_ransac(x1n, x2n, model, maxiter=5000, match_threshold=1e-1)
print("F:", F)
P1 = array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
P2 = sfm.compute_P_from_fundamental(F)
print("P2:", P2)
# triangulate inliers and remove points not in front of both cameras
X = sfm.triangulate(x1n[:, inliers], x2n[:, inliers], P1, P2)
# plot the projection of X
cam1 = camera.Camera(P1)
cam2 = camera.Camera(P2)
x1p = cam1.project(X)
x2p = cam2.project(X)
figure(figsize=(16, 16))
imj = sift.appendimages(im1, im2)
imj = vstack((imj, imj))
imshow(imj)
cols1 = im1.shape[1]
rows1 = im1.shape[0]
for i in range(len(x1p[0])):
if (0<= x1p[0][i]效果图：
基础矩阵F：
投影矩阵P1、P2：
（1）一个简单的数据集
想要从书上给的网址 http://www.robots.ox.ac.uk/~vgg/data/data-mview.html 中下载牛津多视图数据集下载Merton2数据的压缩文件。下面的脚本可以加载Merton2的数据：
编写实验代码：
# -*- coding: utf-8 -*-
import camera
import numpy as np
from PIL import Image
from pylab import *
im1 = array(Image.open('picture/images (1)/001.jpg'))
im2 = array(Image.open('picture/images (1)/002.jpg'))
points2D = [loadtxt('picture/2D (1)/00'+str(i+1)+'.corners').T for i in range(3)]
points3D = loadtxt('picture/3D (1)/p3d').T
corr = genfromtxt('picture/2D (1)/nview-corners',dtype='int',missing_values='*')
P = [camera.Camera(loadtxt('picture/2D (1)/00'+str(i+1)+'.P')) for i in range(3)]
# make 3D points homogeneous and project
X = vstack((points3D,ones(points3D.shape[1])))
x = P[0].project(X)
# plotting the points in view 1
figure()
imshow(im1)
plot(points2D[0][0],points2D[0][1],'*')
axis('off')
figure()
imshow(im1)
plot(x[0],x[1],'r.')
axis('off')
show()
效果图：
实验分析：
仔细观察这些点的分布情况可知，图像点和三维点有些点是重合的，但是并不是完全重合。蓝色这些点代表的是Merton2数据集中的点，红色的点代表的是特征映射过来的坐标。如下图所示。
（2）用matplotlib绘制三维数据
为了可视化三维重建结果，需要绘制出三维图像。Matplotlib中的mplot3d工具包可以绘制出三维点、线、等轮廓线、表面以及其他基本图形组件，还可以通过图像窗口控件实现三维旋转和缩放。
根据上面Merton2下载的数据信息，绘制出该图像的三维点图，编写代码如下：
# -*- coding: utf-8 -*-
from mpl_toolkits.mplot3d import axes3d
fig = figure()
ax = fig.gca(projection="3d")
ax.plot(points3D[0],points3D[1],points3D[2],'k.')
#生成三维采样点
X,Y,Z = axes3d.get_test_data(0.25)
#在三维中绘制点
ax.plot(X.flatten(),Y.flatten(),Z.flatten(),'o')
show()
get_test_data() 函数在 x, y 空间按照设定的空间间隔参数来产生均匀的采样点。压平这些网格，会产生三列数据点，然后我们可以将其输入 plot() 函数。这样，我们就可以在立体表面上画出三维点。
效果图：
现在通过画出Merton样本数据来观察三维点的效果，运行代码：
# -*- coding: utf-8 -*-
from pylab import *
from mpl_toolkits.mplot3d import axes3d
import matplotlib.pyplot as pt
plt.rcParams['font.sans-serif']=['SimHei'] # 用来正常显示中文标签
plt.rcParams['axes.unicode_minus']=False # 用来正常显示负号
fig = figure()
ax = fig.gca(projection="3d")
ax.plot(points3D[0],points3D[1],points3D[2],'k.')
show()
运行后的效果图：
- 初始显示的三维点图
- 俯视的视图，展示了建筑墙体和屋顶上的点
分析：
通过旋转上面的坐标系，在不同三维角度显示这些三维数据点的分布。
（3）计算F：八点法
八点法是通过对应点来计算基础矩阵的算法。
八点算法的原理：
由于基础矩阵 F F F定义为：
x T F x ′ = 0 x^{T}F{x}'=0 xTFx′=0
任给两幅图像中的匹配点 x x x 与 x ′ x' x′ ，令 x=(u,v,1)T ，x’=(u’,v’,1)T，基础矩阵F是一个3×3的秩为2的矩阵，一般记基础矩阵F为：
F = [ f 11 f 12 f 13 f 21 f 22 f 23 f 31 f 32 f 33 ] F=\begin{bmatrix} f_{11} & f_{12} & f_{13}\\ f_{21} & f_{22} & f_{23}\\ f_{31} & f_{32} & f_{33} \end{bmatrix} F=⎣⎡f11f21f31f12f22f32f13f23f33⎦⎤
有相应方程：
u u ′ f 11 + u v ′ f 12 + u f 13 + v u ′ f 21 + v v ′ f 22 + v f 23 + u ′ f 31 + v ′ f 32 + f 33 = 0 u{u}'f_{11}+u{v}'f^{12}+uf^{13}+v{u}'f^{21}+v{v}'f^{22}+vf^{23}+{u}'f_{31}+{v}'f_{32}+f_{33}=0 uu′f11+uv′f12+uf13+vu′f21+vv′f22+vf23+u′f31+v′f32+f33=0
由矩阵乘法可知有：
在实际计算中，可以直接用 A T A A^{T}A ATA的分解来求解参数。 也可以用非线性优化，通过搜索f使得 ∣ ∣ A f ∣ ∣ ||Af|| ∣∣Af∣∣最小化, 同时满足 ∣ ∣ f ∣ ∣ = 1 ||f||=1 ∣∣f∣∣=1的约束。上述求解后的 F F F不一定能满足秩为2的约束，因此 还要在 F F F基础上加以约束。
通过SVD分解可以解决上述问题，令 F = U Σ V T F=U\Sigma V^{T} F=UΣVT则 ：
Σ = [ σ 1 0 0 0 σ 2 0 0 0 σ 3 ] \Sigma =\begin{bmatrix} \sigma _{1} & 0 & 0\\ 0 & \sigma _{2} & 0\\ 0 & 0 & \sigma _{3} \end{bmatrix} Σ=⎣⎡σ1000σ2000σ3⎦⎤ 令 Σ ′ = [ σ 1 0 0 0 σ 2 0 0 0 0 ] {\Sigma}' =\begin{bmatrix} \sigma _{1} & 0 & 0\\ 0 & \sigma _{2} & 0\\ 0 & 0 & 0 \end{bmatrix} Σ′=⎣⎡σ1000σ20000⎦⎤ 则最终的解为： F ′ = U Σ ′ V T {F}'=U{\Sigma}' V^{T} F′=UΣ′VT
由于基本矩阵有一个重要的特点就是奇异性， F F F矩阵的秩是 2 2 2。如果基本矩阵是非奇异的，那么所计算的对极线将不重合。所以在上述算法解得基本矩阵后，会增加一个奇异性约束。 最简便的方法就是修正上述算法中求得的矩阵 F F F。设最终的解为 F ′ {F}′ F′，令 d e t F ′ = 0 det{F}′=0 detF′=0下求得Frobenius范数（二范数） ∥ F − F ′ ∥ ∥F−F′∥ ∥F−F′∥最小的 F ′ F′ F′。这种方法的实现还是使用了SVD分解，若 F = U D V T F=UDV^{T} F=UDVT，此时的对角矩阵 D = d i a g ( r , s , t ) D=diag(r,s,t) D=diag(r,s,t)，满足 r ≥ s ≥ t r≥s≥t r≥s≥t，则 F ′ = U d i a g ( r , s , 0 ) V T F′=Udiag(r,s,0)V^{T} F′=Udiag(r,s,0)VT最小化范数 ∥ F − F ′ ∥ ∥F−F′∥ ∥F−F′∥，也就是最终的解。
八点算法的基本步骤：
- 求线性解 由系数矩阵 A A A最小奇异值对应的奇异矢量 f f f求的 F F F。
- 奇异性约束 是最小化Frobenius范数 ‖ F − F ′ ‖ ‖F−F′‖ ‖F−F′‖的 F ′ F' F′代替 F F F。
八点算法的优点：
线性求解，容易实现，运行速度快 。
八点算法的缺点：
对噪声敏感。
创建一个sfm.py写入八点法中最小化的函数：
# -*- coding: utf-8 -*-
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，使秩为2
U,S,V = linalg.svd(F)
S[2] = 0
F = dot(U,dot(diag(S),V))
return F
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):
"""在图像中，绘制外极点和外极线 Fx=0 。F是基础矩阵，x是另一幅图像中的点"""
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通常SVD算法来计算最小二乘法，由于上面的算法得出的解可能秩不为2（基础矩阵的秩小于等于2），所以需要通过最后一个奇异值置0来得到秩最接近2的基础矩阵。上面的函数忽略了一个重要的步骤：对图像坐标进行归一化，这可能会带来数值问题。
正如开篇运行的实验代码给出，可以总结出使用八点算法估算F的步骤：
第一步，导入两幅图像，并使用sift算法提取特征；
第二步，使用函数match_twosided连接两幅图的特征；
第三步，RANSAC去除错误点匹配；
第四步，归一化8点算法估计基础矩阵。这是通过对应点来计算基础矩阵的算法。
修改开篇的实验代码：
# -*- coding: utf-8 -*-
from PIL import Image
from numpy import *
from pylab import *
import numpy as np
from PCV.geometry import camera
from PCV.geometry import homography
from PCV.geometry import sfm
from PCV.localdescriptors import sift
# -*- coding: utf-8 -*-
im1 = array(Image.open('picture/home/1.jpg'))
im2 = array(Image.open('picture/home/2.jpg'))
sift.process_image('picture/home/1.jpg', 'im1.sift')
l1, d1 =sift.read_features_from_file('im0.sift')
sift.process_image('picture/home/2.jpg', 'im2.sift')
l2, d2 =sift.read_features_from_file('im1.sift')
matches = sift.match_twosided(d1, d2)
ndx = matches.nonzero()[0]
x1 = homography.make_homog(l1[ndx, :2].T)
ndx2 = [int(matches[i]) for i in ndx]
x2 = homography.make_homog(l2[ndx2, :2].T)
d1n = d1[ndx]
d2n = d2[ndx2]
x1n = x1.copy()
x2n = x2.copy()
figure(figsize=(16,16))
sift.plot_matches(im1, im2, l1, l2, matches, True)
show()
def F_from_ransac(x1, x2, model, maxiter=5000, match_threshold=1e-6):
""" Robust estimation of a fundamental matrix F from point
correspondences using RANSAC (ransac.py from
http://www.scipy.org/Cookbook/RANSAC).
input: x1, x2 (3*n arrays) points in hom. coordinates. """
from PCV.tools import ransac
data = np.vstack((x1, x2))
d = 10 # 20 is the original
# compute F and return with inlier index
F, ransac_data = ransac.ransac(data.T, model,
8, maxiter, match_threshold, d, return_all=True)
return F, ransac_data['inliers']
# find E through RANSAC
model = sfm.RansacModel()
F, inliers = F_from_ransac(x1n, x2n, model, maxiter=5000, match_threshold=1e-4)
print(len(x1n[0]))
print(len(inliers))
# 计算照相机矩阵（P2 是 4 个解的列表）
P1 = array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
P2 = sfm.compute_P_from_fundamental(F)
# triangulate inliers and remove points not in front of both cameras
X = sfm.triangulate(x1n[:, inliers], x2n[:, inliers], P1, P2)
# plot the projection of X
cam1 = camera.Camera(P1)
cam2 = camera.Camera(P2)
x1p = cam1.project(X)
x2p = cam2.project(X)
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()
figure(figsize=(16, 16))
im3 = sift.appendimages(im1, im2)
im3 = vstack((im3, im3))
imshow(im3)
cols1 = im1.shape[1]
rows1 = im1.shape[0]
for i in range(len(x1p[0])):
if (0<= x1p[0][i]0: #plot([locs1[i][0],locs2[m][0]+cols1],[locs1[i][1],locs2[m][1]],'c')
x1=int(l1[i][0])
y1=int(l1[i][1])
x2=int(l2[int(m)][0])
y2=int(l2[int(m)][1])
# p1 = array([l1[i][0], l1[i][1], 1])
# p2 = array([l2[m][0], l2[m][1], 1])
p1 = array([x1, y1, 1])
p2 = array([x2, y2, 1])
# Use Sampson distance as error
Fx1 = dot(F, p1)
Fx2 = dot(F, p2)
denom = Fx1[0]**2 + Fx1[1]**2 + Fx2[0]**2 + Fx2[1]**2
e = (dot(p1.T, dot(F, p2)))**2 / denom
x1e.append([p1[0], p1[1]])
x2e.append([p2[0], p2[1]])
ers.append(e)
x1e = array(x1e)
x2e = array(x2e)
ers = array(ers)
indices = np.argsort(ers)
x1s = x1e[indices]
x2s = x2e[indices]
ers = ers[indices]
x1s = x1s[:20]
x2s = x2s[:20]
figure(figsize=(16, 16))
im3 = sift.appendimages(im1, im2)
im3 = vstack((im3, im3))
imshow(im3)
cols1 = im1.shape[1]
rows1 = im1.shape[0]
for i in range(len(x1s)):
if (0<= x1s[i][0] SIFT对两个图像进行特征提取以及匹配，然后使用归一化8点算法进行基本矩阵的求解，并且把两个视图的对极线都画出。
效果图：
ransac算法结果为：
八点算法结果为：
基本矩阵为：
（4）外极点和外极线
外极点满足 F e 1 = 0 Fe_{1}=0 Fe1=0，因此可以通过计算 F F F的零空间来得到。如果想要获得另一幅图像的外极点（对应左零空间的外极点），只需将 F F F转置后输入到函数def compute_epipole(F) 中即可。
可以将之前样本数据集的两个视图上运行这个函数：
# -*- coding: utf-8 -*-
from pylab import *
from mpl_toolkits.mplot3d import axes3d
import sfm
#在前两个视图中点的索引
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 = sfm.compute_fundamental(x1,x2)
#计算极点
e = sfm.compute_epipole(F)
#绘制图像
figure()
imshow(im1)
#分别绘制每条线，这样会绘制出很漂亮的颜色
for i in range(5):
sfm.plot_epipolar_line(im1,F,x2[:,i],e,False)
axis('off')
figure()
imshow(im2)
#分别绘制每个点，这样绘制出和线同样的颜色
for i in range(5):
plot(x2[0,i],x2[1,i],'o')
axis('off')
show()
效果图：
实验分析：
选择两幅图像的对应点，然后将它们转换为齐次坐标，这里的对应点是从一个文本文件中读取得到的；而实际上，可以按照sift提取图像特征的方式，然后通过匹配来找到它们。由于缺失的数据在对应列表corr中为-1，所以程序中有可能选取这些点。因此，上面的程序通过数组操作符&只选取了索引大于等于0的点。最后，在第一个视图中画出了前5条外极线，在第二个视图中画出了对应5个匹配点。还借助了plot_epipolar_line()函数，这个函数将x轴的范围作为直线的参数，所以直线超出了图像边界的部分会被截断。如果show_epipole为真，外极点会被画出来（如果输入参数中没有外极点，外极点会在程序中计算获得）。用不同的颜色将点和对应的外极线对应起来。
（二）照相机和三维结构的计算
（1）三角剖分
1.三角剖分
给定照相机参数模型，图像点可以通过三角剖分来恢复出这些点的三维位置。
数学定义： 假设V是二维实数域上的有限点集，边e是由点集中的点作为端点构成的封闭线段, E为e的集合。那么该点集V的一个三角剖分T=(V,E)是一个平面图G，该平面图满足条件：
a.除了端点，平面图中的边不包含点集中的任何点。
b.没有相交边。
c.平面图中所有的面都是三角面，且所有三角面的合集是散点集V的凸包。
对于两个照相机P1和P2的视图，三维实物点X的投影点为x1和x2（用齐次坐标表示），照相机方程定义了下面的关系：
[ P 1 − x 1 0 P 2 0 − x 2 ] [ X λ 1 λ 2 ] = 0 \begin{bmatrix} P_{1} & -x_{1} & 0\\ P_{2} & 0 & -x_{2} \end{bmatrix}\begin{bmatrix} X\\ \lambda _{1}\\ \lambda _{2} \end{bmatrix}=0 [P1P2−x100−x2]⎣⎡Xλ1λ2⎦⎤=0
由于图像噪声、照相机参数误差和其他系统误差，上面的方程可能没有精确解。可以通过SVD算法来得到三维点的最小二乘估值。
将triangulate_point()和triangulate()函数添加到sfm.py中：
def triangulate_point(x1,x2,P1,P2):
"""使用最小二乘解，绘制点对的三角剖分"""
M = zeros((6,6))
M[:3,:4] = P1
M[3:,:4] = P2
M[:3,4] = -x1
M[3:,5] = -x2
U,S,V = linalg.svd(M)
X = V[-1, :4]
return X / X[3]
def triangulate(x1, x2, P1, P2):
"""X1和X2（3*n的齐次坐标表示）中点的二视图三角剖分"""
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
第一个函数用于计算一个点对的最小二乘三角剖分，它的最后一个特征向量的前4个值就是齐次坐标系下的对应三维坐标。第二个函数用于实现多个点的三角剖分，这个函数的输入是两个图像点数组，输出为一个三维坐标数组。
利用下面的代码来实现Merton2数据集上的三角剖分：
# -*- coding: utf-8 -*-
from pylab import *
from mpl_toolkits.mplot3d import axes3d
import sfm
# 在前两个视图中点的索引
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])))
Xtrue = points3D[:,ndx]
Xtrue = vstack( (Xtrue,ones(Xtrue.shape[1])) )
# 检查前三个点
Xest = sfm.triangulate(x1,x2,P[0].P,P[1].P)
print (Xest[:,:3])
print (Xtrue[:,:3])
# 绘制图像
fig = figure()
ax = fig.gca(projection='3d')
ax.plot(Xest[0],Xest[1],Xest[2],'ko')
ax.plot(Xtrue[0],Xtrue[1],Xtrue[2],'r.')
axis('equal')
show()
效果图：
上面的代码首先利用前两个视图的信息来对图像点进行三角剖分，然后把前三个图像点的齐次坐标输出到控制台，最后绘制出恢复的最接近三维图像点。输出到控制台的信息如下所示：
这个算法估计出的三维图像点和实际图像点很接近。估计点和实际点可以很好地进行匹配。
（2）由三维点计算照相机矩阵
如果已经知道了一些三维点及其图像投影，我们可以使用直接线性变换的方法来计算照相机矩阵 P P P。本质上，这是三角剖分方法的逆问题，有时将其称为照相机反切法。利用该方法恢复照相机矩阵同样也是一个最小二乘问题。
从照相机方程可以得出，每个三维点 X i X_{i} Xi（齐次坐标系下）按照 λ i x i = P X i \lambda _{i}x_{i}=PX_{i} λixi=PXi 投影到图像点 x i = [ x i , y i , 1 ] x_{i}=[x_{i},y_{i},1] xi=[xi,yi,1]，相应的点满足下面的关系：
[ X 1 T 0 0 − x 1 0 0 . . . 0 X T 1 0 − y 1 0 0 . . . 0 0 X 1 T − 1 0 0 . . . X 2 T 0 0 0 − x 2 0 . . . 0 X 2 T 0 0 − y 2 0 . . . 0 0 X 2 T 0 − 1 0 . . . ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ] [ p 1 T p 2 T p 3 T λ 1 λ 2 ⋮ ] = 0 \begin{bmatrix} X^{T}_{1} & 0 & 0 & -x_{1} & 0 & 0 & ...\\ 0& X{T}_{1} & 0 & -y_{1} & 0 & 0 & ...\\ 0 & 0 & X^{T}_{1} & -1 & 0 & 0 & ...\\ X^{T}_{2} & 0 & 0 & 0 & -x_{2} & 0 & ...\\ 0 & X^{T}_{2} & 0 & 0 & -y_{2} & 0 & ...\\ 0 & 0 & X^{T}_{2} & 0 & -1 & 0 & ...\\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \end{bmatrix}\begin{bmatrix} p^{T}_{1}\\ p^{T}_{2}\\ p^{T}_{3}\\ \lambda _{1}\\ \lambda _{2}\\ \vdots \end{bmatrix}=0 ⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡X1T00X2T00⋮0XT100X2T0⋮00X1T00X2T⋮−x1−y1−1000⋮000−x2−y2−1⋮000000⋮..................⋮⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎡p1Tp2Tp3Tλ1λ2⋮⎦⎥⎥⎥⎥⎥⎥⎥⎤=0
其中 p1、p2和p3是矩阵 P P P的三行。上面的式子可以写得更紧凑，如下所示： M v = 0 Mv=0 Mv=0
可以使用SVD分解估计出照相机矩阵。利用上面讲述的矩阵操作，直接编写实验代码。
下面在之前使用的样本数据集上测试算法的性能。下面编写的代码会选出第一个视图中的可见点（使用对应列表中缺失的数值），将它们转换成齐次坐标表示，然后估计照相机矩阵：
# -*- coding: utf-8 -*-
import camera
import numpy as np
from PIL import Image
from pylab import *
im1 = array(Image.open('picture/images (1)/001.jpg'))
im2 = array(Image.open('picture/images (1)/002.jpg'))
points2D = [loadtxt('picture/2D (1)/00'+str(i+1)+'.corners').T for i in range(3)]
points3D = loadtxt('picture/3D (1)/p3d').T
corr = genfromtxt('picture/2D (1)/nview-corners',dtype='int',missing_values='*')
P = [camera.Camera(loadtxt('picture/2D (1)/00'+str(i+1)+'.P')) for i in range(3)]
corr = corr[:,0] # 视图 1
ndx3D = where(corr>=0)[0] # 丢失的数值为 -1
ndx2D = corr[ndx3D]
# 选取可见点，并用齐次坐标表示
x = points2D[0][:,ndx2D] # 视图 1
x = vstack( (x,ones(x.shape[1])) )
X = points3D[:,ndx3D]
X = vstack( (X,ones(X.shape[1])) )
# 估计 P
Pest = camera.Camera(sfm.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()
实验效果：
输出结果为：
上面是估计出的照相机矩阵，下面是数据集的创建者计算出的照相机矩阵。可以看出，它们的元素几乎完全相同。最后使用估计出的照相机矩阵投影这些三维点，然后绘制出投影后的结果。
真实点用圆圈表示，估计出的照相机投影点用点表示。
（3）由基础矩阵计算照相机矩阵
在两个视图场景中，照相机矩阵可以由基础矩阵恢复出来。假设第一个照相机矩阵归一化为P1=[I/0]，然后需要计算出第二个照相机矩阵P2。分成两类情况，一类是未标定，另一类是已标定。
- 未标定的情况——投影重建
在没有任何照相机内参数知识的情况下，照相机矩阵只能通过射影变换恢复出来。也就是说，如果利用照相机的信息来重建三维点，那么该重建只能由射影变换计算出来（可以得到整个投影场景中无畸变的重建点）。在这里，不考虑角度和距离。
定义： 在无标定的情况下，第二个照相机矩阵可以使用一个（3×3）的射影变换得到。
一个简单的方法就是： P 2 = [ S e F ∣ e ] P_{2}=[S_{e}F|e] P2=[SeF∣e] 其中， e e e是左极点，满足 e T F = 0 e^{T}F=0 eTF=0， S e S_{e} Se是公式所示的反对称矩阵。需要注意的是，使用该矩阵做出的三角形剖分很有可能会发生畸变，如倾斜的重建。
下面是具体的代码，把它添加到sfm.py中：
def compute_P_from_fundamental(F):
"""从基础矩阵中计算第二个照相机矩阵（假设 P1 = [I 0])"""
e = compute_epipole(F,T) #左极点
Te = skew(e)
return vstack((dot(Te,F.T).T,e)).T
def skew(a):
"""反对称矩阵A，使得对于每个v有a×v=Av"""
return array([[0,-a[2],a[1]],[a[2],0,-a[0]],[-a[1],a[0],0]])
- 已标定的情况——度量重建
在已经标定的情况下，重建会保持欧式空间中的一些度量特性（除了全局的尺度参数）。
定义： 给定标定矩阵 K K K，可以将它的逆 K − 1 K^{-1} K−1 作用于图像点 x K = K − 1 x x_{K}=K^{-1}x xK=K−1x，因此，在新的图像坐标系下，照相机方程变为： x K = K − 1 K [ R ∣ t ] X = [ R ∣ t ] X x_{K}=K^{-1}K[R|t]X=[R|t]X xK=K−1K[R∣t]X=[R∣t]X
在新的图像坐标系下，点同样满足之前的基础矩阵方程： x K 2 T F x K 1 = 0 x_{K2}^{T}Fx_{K1}=0 xK2TFxK1=0
在标定归一化的坐标系下，基础矩阵称为本质矩阵。为了区别为标定后的情况，以及归一化了的图像坐标，通常将其记为 E E E，而非 F F F。
从本质矩阵中恢复出的照相机矩阵中存在度量关系，但有四个可能解。因为只有一个解产生位于两个照相机前的场景，所以可以从中选出来。
下面是具体计算4个解的代码，把它添加到sfm.py中：
def compute_P_from_essential(E):
"""从本质矩阵中计算第二个照相机矩阵（假设 P1 = [I 0])
输出为4个可能的照相机矩阵列表"""
# 保证E的秩为2
U, S, V = svd(E)
if det(dot(U, V)) < 0:
V = -V
E = dot(U, dot(diag([1, 1, 0]), V))
# 创建矩阵（Hartley）
Z = skew([0, 0, -1])
W = array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
# 返回所有（4个）解
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
了解完上面的公式和代码，介绍一下实验步骤：
- 用SIFT算法实现两幅图像的特征点检测，找到对应的匹配点并绘制出来；
- 使用RANSAC方法估计最佳基础矩阵F，以及正确点的索引，求出不同图像对的基础矩阵；
- 由基础矩阵计算照相机矩阵；
- 从照相机矩阵的列表中，对正确点的三维点进行三角剖分，挑选出经过三角剖分后，在两个照相机前均含有最多场景点。
编写实验代码：
# -*- coding: utf-8 -*-
# -*- coding: utf-8 -*-
from PIL import Image
from numpy import *
from pylab import *
import numpy as np
from PCV.geometry import camera
import homography
from PCV.geometry import sfm
from PCV.localdescriptors import sift
from imp import reload
camera = reload(camera)
homography = reload(homography)
sfm = reload(sfm)
sift = reload(sift)
# Read features
im1 = array(Image.open('picture/home/1.jpg'))
im2 = array(Image.open('picture/home/2.jpg'))
sift.process_image('picture/home/1.jpg','im1.sift')
l0, d0 = sift.read_features_from_file('im1.sift')
sift.process_image('picture/home/2.jpg','im2.sift')
l1, d1 = sift.read_features_from_file('im2.sift')
matches = sift.match_twosided(d1, d2)
ndx = matches.nonzero()[0]
x1 = homography.make_homog(l1[ndx, :2].T)
ndx2 = [int(matches[i]) for i in ndx]
x2 = homography.make_homog(l2[ndx2, :2].T)
d1n = d1[ndx]
d2n = d2[ndx2]
x1n = x1.copy()
x2n = x2.copy()
figure(figsize=(16, 16))
sift.plot_matches(im1, im2, l1, l2, matches, True)
show()
# def F_from_ransac(x1, x2, model, maxiter=5000, match_threshold=1e-6):
def F_from_ransac(x1, x2, model, maxiter=5000, match_threshold=1e-6):
""" Robust estimation of a fundamental matrix F from point
correspondences using RANSAC (ransac.py from
http://www.scipy.org/Cookbook/RANSAC).
input: x1, x2 (3*n arrays) points in hom. coordinates. """
from PCV.tools import ransac
data = np.vstack((x1, x2))
d = 10 # 20 is the original
# compute F and return with inlier index
F, ransac_data = ransac.ransac(data.T, model,
8, maxiter, match_threshold, d, return_all=True)
return F, ransac_data['inliers']
# find F through RANSAC
model = sfm.RansacModel()
F, inliers = F_from_ransac(x1n, x2n, model, maxiter=5000, match_threshold=1e-3)
print (F)
P1 = array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
P2 = sfm.compute_P_from_fundamental(F)
print (P2)
print (F)
# triangulate inliers and remove points not in front of both cameras
X = sfm.triangulate(x1n[:, inliers], x2n[:, inliers], P1, P2)
# plot the projection of X
cam1 = camera.Camera(P1)
cam2 = camera.Camera(P2)
x1p = cam1.project(X)
x2p = cam2.project(X)
figure(figsize=(16, 16))
imj = sift.appendimages(im1, im2)
imj = vstack((imj, imj))
imshow(imj)
cols1 = im1.shape[1]
rows1 = im1.shape[0]
for i in range(len(x1p[0])):
if (0 <= x1p[0][i] < cols1) and (0 <= x2p[0][i] < cols1) and (0 <= x1p[1][i] < rows1) and (0 <= x2p[1][i] < rows1):
plot([x1p[0][i], x2p[0][i] + cols1], [x1p[1][i], x2p[1][i]], 'c')
axis('off')
show()
d1p = d1n[inliers]
d2p = d2n[inliers]
# Read features
im3 = array(Image.open('picture/home/3.jpg'))
sift.process_image('picture/home/3.jpg','im3.sift')
l3, d3 = sift.read_features_from_file('im3.sift')
matches13 = sift.match_twosided(d1p, d3)
ndx_13 = matches13.nonzero()[0]
x1_13 = homography.make_homog(x1p[:, ndx_13])
ndx2_13 = [int(matches13[i]) for i in ndx_13]
x3_13 = homography.make_homog(l3[ndx2_13, :2].T)
figure(figsize=(16, 16))
imj = sift.appendimages(im1, im3)
imj = vstack((imj, imj))
imshow(imj)
cols1 = im1.shape[1]
rows1 = im1.shape[0]
for i in range(len(x1_13[0])):
if (0 <= x1_13[0][i] < cols1) and (0 <= x3_13[0][i] < cols1) and (0 <= x1_13[1][i] < rows1) and (
0 <= x3_13[1][i] < rows1):
plot([x1_13[0][i], x3_13[0][i] + cols1], [x1_13[1][i], x3_13[1][i]], 'c')
axis('off')
show()
P3 = sfm.compute_P(x3_13, X[:, ndx_13])
print (P3)
print (P1)
print (P2)
print (P3)
实验效果及分析：
对两张图片进行sift特征提取与匹配并生成sift文件：
sift特征提取与匹配出来的效果图：（室外和室内）
计算出的基础矩阵F和第二个照相机矩阵P2的结果如下：
室外：
室内：
三角剖分后，得到的第一张图和第二张图均含有最多场景点的匹配结果图如下：
三角剖分后，得到的第一张图和第三张图均含有最多场景点的匹配结果图如下：
计算出第一个照相机矩阵p1，第二个照相机矩阵p2，第三照相机矩阵p3的值如下：
室外：
室内：
由上面室外的结果看出，第一个照相机位置和第二个照相机位置匹配的结果相比之下，效果比较好；而第一个照相机位置和第三个照相机为止的匹配效果并没那么好，匹配的点比较少。从室内的结果看出，照相机的位置和图像呈现的匹配点有关系，拍摄的角度和清晰度会对图像匹配产生影响。
在运行室内代码的时候，出现了如下的错误：
这是因为这段代码在计算单应性矩阵时，每个图像对中，匹配是从最右边的图像计算出来的，如果你的第二张与第一张图片没有共同点， 会错乱拼接图片或者出现ValueError: did not meet fit acceptance criteria错误。所以你在给图片命名时，要按照从右到左的顺序命名。
（三）多视图重建
下面介绍如何使用从多幅图像中计算出真实的三维重建。由于照相机的运动给我们提供了三维结构，所以这样计算三维重建的方法通常称为SFM（从运动中恢复结构）。
假设照相机已经标定，计算重建可以分为下面的4个步骤：
- 检测特征点，然后在两幅图像间匹配；
- 由匹配计算基础矩阵；
- 由基础矩阵计算照相机矩阵；
- 三角剖分这些三维点。
当前已经具备了完成上面4个步骤所需的所有工具，但是当图像间的点对应包含不正确的匹配时，需要一个稳健的方法来计算基础矩阵。
（1）稳健估计基础矩阵
类似之前稳健计算单应性矩阵，当存在噪声和不正确的匹配时，需要估计基础矩阵。 使用RANSAC方法，结合了八点算法。需要注意的是，八点算法在平面场景中会失效。所以，如果场景点都位于平面上，就不能使用这种算法。
这个需要将下面的代码添加到sfm.py文档中即可：
def F_from_ransac(x1,x2,model,maxiter=5000,match_theshold=1e-6):
"""使用RANSAC方法，从点对应中稳健地估计基础矩阵F
输入：使用齐次坐标表示的点x1,x2（3*n的数组）"""
import ransac
data = vstack((x1,x2))
#计算F,并返回正确点索引
F,ransac_data = ransac.ransac(data.T,model,8,maxiter,match_theshold,20,return_all=True)
return F,ransac_data['inliers']
def compute_fundamental_normalized(x1,x2):
"""使用归一化的八点算法，由对应点（x1,x2 3*n 的数组）计算基础矩阵"""
n = x1.shape[1]
if x2.shape[1]!=n:
raise ValueError("Number of points don't match.")
#归一化图像坐标
x1 = x1/x1[2]
mean_1 = mean(x1[:2],axis=1)
S1 = sqrt(2)/std(x1[:2])
T1 = array([[S1,0,-S1*mean_1[0]],[0,S1,-S1*mean_1[1]],[0,0,1]])
x1 = dot(T1,x1)
x2 = x2/x2[2]
mean_2 = mean(x2[:2],axis=1)
S2 = sqrt(2)/std(x2[:2])
T2 = array([[S2,0,-S2*mean_2[0]],[0,S2,-S2*mean_2[1]],[0,0,1]])
x2 = dot(T2,x2)
#使用归一化的坐标计算F
F = compute_fundamental(x1,x2)
#反归一化
F = dot(T1.T,dot(F,T2))
return F/F[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):
"""使用选择的8个对应计算基础矩阵"""
#转置，并将数据分成两个点集
data = data.T
x1 = data[:3,:8]
x2 = data[3:,:8]
#估算基础矩阵，并返回
F = compute_fundamental_normalized(x1,x2)
return F
def get_error(self,data,F):
"""计算所有对应的x^T F X，并返回每个变换后点的误差"""
#转置，并将数据分成两个点集
data = data.T
x1 = data[:3]
x2 = data[3:]
#将sampson距离用作误差度量
Fx1 = dot(F,x1)
Fx2 = dot(F,x2)
denom = Fx1[0]**2 + Fx1[1]**2 + Fx2[0]**2 +Fx2[1]**2
err = (diag(dot(x1.T,dot(F,x2)))) **2/denom
#返回每个点的误差
return err
其中，compute_fundamental_normalized()函数将图像点归一化为零均值固定方差。
（2）三维重建示例
编写实验代码：
# -*- coding: utf-8 -*-
from PIL import Image
from numpy import *
from pylab import *
import numpy as np
import homography
import sfm
import sift
# 标定矩阵
K = array([[2394,0,932],[0,2398,628],[0,0,1]])
# 载入图像，并计算特征
im1 = array(Image.open('picture/home/1.jpg'))
im2 = array(Image.open('picture/home/2.jpg'))
sift.process_image('picture/home/1.jpg','im1.sift')
l0, d0 = sift.read_features_from_file('im1.sift')
sift.process_image('picture/home/2.jpg','im2.sift')
l1, d1 = sift.read_features_from_file('im2.sift')
# 匹配特征
matches = sift.match_twosided(d1,d2)
ndx = matches.nonzero()[0]
# 使用齐次坐标表示，并使用inv(K)归一化
x1 = homography.make_homog(l1[ndx,:2].T)
ndx2 = [int(matches[i]) for i in ndx]
x2 = homography.make_homog(l2[ndx2,:2].T)
x1n = dot(inv(K),x1)
x2n = dot(inv(K),x2)
# 使用RANSAC方法估计E
model = sfm.RansacModel()
E,inliers = sfm.F_from_ransac(x1n,x2n,model)
# 计算照相机矩阵（P2是4个解的列表）
P1 = array([[1,0,0,0],[0,1,0,0],[0,0,1,0]])
P2 = sfm.compute_P_from_essential(E)
# 选取点在照相机前的解
ind = 0
maxres = 0
for i in range(4):
# 三角剖分正确点，并计算每个照相机的深度
X = sfm.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 = i
infront = (d1>0) & (d2>0)
# 三角剖分正确点，并移除不在所有照相机前面的点
X = sfm.triangulate(x1n[:,inliers],x2n[:,inliers],P1,P2[ind])
X = X[:,infront]
# 绘制三维图像
from mpl_toolkits.mplot3d import axes3d
fig = figure()
ax = fig.gca(projection='3d')
ax.plot(-X[0], X[1], X[2], 'k.')
axis('off')
# 绘制X的投影 import camera
# 绘制三维点
cam1 = camera.Camera(P1)
cam2 = camera.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()
实验结果：
ransac算法算出的模型参数：
（3）多视图的扩展示例
多视图重建有一些步骤和进一步的扩展，下面提供关于一些有关内容的介绍。
- 多视图
当我们有同一场景的多个视图时，三维重建会变得更加准确，包括更多的细节信息。因为基本矩阵只和一对视图相关，所以该过程带有很多图像，和之前的处理有些不同。
对于视频序列， 可是使用时序关系，在连续的帧对中匹配特征。相对方位需要从每个帧对增量地加入下一个帧对。同时可以使用跟踪有效地找到对应点。
对于静止的图像， 一个办法是找到一个中央参考视图，然后计算与之有关的所有其他照相机矩阵。另一个办法是计算一个图像对的照相机矩阵和三维重建，然后增量地加入新的图像和三维点。另外，还有一些方法可以同时由三个视图计算三维重建和照相机位置。 - 光束法平差
恢复出的点的位置和由估计的基础矩阵计算出的照相机矩阵都存在误差。在多个视图的计算中，这些误差会进一步累积。因此，多视图重建的最后一步，通常是通过优化三维点的位置和照相机参数来减少二次投影误差 。 该过程称为光束法平差。 - 自标定
在未标定照相机的情况中，有时可以从图像特征来计算标定矩阵。 该过程称为自标定。根据在照相机标定矩阵参数上做出的假设，以及可用的图像数据的类型（特征匹配、平行线、平面等），根据不同的自标定算法来进行标定。
（四）立体图像
一个多视图成像的特殊例子是立体视觉（或者立体成像），即使用两台只有水平（向一侧）偏移的照相机观测同一场景。当照相机的位置如上设置，两幅图像具有相同的图像平面，图像的行是垂直对齐的，那么称图像对是经过矫正的。该设置在机器人学中很常见，称为立体平台。
通过将图像扭曲到公共平面上，使外极线位于图像行上，任何立体照相机设置都能得到矫正（通常构建立体平台来产生经过矫正的图像对）。
假设两幅图像经过了矫正，那么对应点的寻找限制在图像的同一行上，一旦找到对应点，由于深度是和偏移成正比的，那么深度（Z坐标）可以直接由水平偏移来计算：
Z = f b x l − x r Z=\frac{fb}{x_{l}-x_{r}} Z=xl−xrfb 其中， f f f是经过矫正图像的焦距， b b b是两个照相机中心之间的距离， x l x_{l} xl和 x r x_{r} xr左右两幅图像中对应点的x坐标。分开照相机中心的距离称为基线。
立体重建就是恢复深度图（或者相反，视差图），图像中每个像素的深度都需要计算出来。