Python计算机视觉编程第五章 多视图几何

本章讲解如何处理多个视图，以及如何利用多个视图的集合关系来恢复照相机位置信息和三维结构。通过在不同视点拍摄的图像，我们可以利用特征匹配来计算出三维场景点以及照相机位置。本章会介绍一些基本的方法，展示一个三维重建的完整例子；本章最后将介绍如何由立体图像进行致密深度重建。

1 外极几何

多视图几何是利用在不同视点所拍摄图像间的关系，来研究照相机之间或者特征之间关系的一门学科。图像的特征通常是兴趣点，本章使用的也是兴趣点特征，多视图几何中最重要的内容是双视图几何。

如果有一个场景的两个视图以及视图中对应图像点，那么根据照相机间的空间相对位置关系、照相机的性质以及三维场景点的位置，可以得到对这些图像点的一些几何关系约束。我们通过外极几何来描述这些几何关系。

没有关于照相机的先验知识，会出现固有二义性，因为三维场景点X经过4×4的单应性矩阵H变换为HX后，则HX在照相机PH^-1里得到的图像点和X在照相机P里得到的图像点相同。利用照相机方程，可以将上述问题描述为：$$\lambda x=PX=P{H}^{-1}HX=\hat{P}\hat{X}$$因此，当我们分析双视图几何关系时，总是可以将照相机间的相对位置关系用单应性矩阵加以简化。这里的单应性矩阵通常只做刚体变换，即只通过单应矩阵变换了坐标系。一个比较好的做法是，将原点和坐标轴与第一个照相机对齐，则：$$P_1=K_1[I|0]和P_2=K_2[R|T]$$qi其中K₁和K₂是标定矩阵，R是第二个照相机的旋转矩阵，t是第二个照相机的平移向量。利用这些照相机参数矩阵，我们可以找到点X的投影点x₁和x₂（分别对应于投影矩阵P₁和P₂）的关系。这样，我们可以从寻找对应的图像出发，恢复照相机参数矩阵。

同一个图像点经过不同的投影矩阵产生的不同投影点必须满足：$$x_2^{T}F{x}_1=0$$其中：$$F=K_2^{-T}{S}_{t}R{K}_1^{-1}$$矩阵S_t为反对称矩阵：$$S_t=\left[\begin{array}{ccc}0& -t_3& t_2\\ t_3& 0& -t_1\\ -t_2& t_1& 0\end{array}\right]$$上述为外极约束条件。矩阵F为基础矩阵。基础矩阵可以由两照相机的参数矩阵（相对旋转R和平移t）表示。由于det(F)=0，所以基础矩阵的秩小于等于2。我们将在估计F的算法中用到这些性质。

上面的公式表明，我们可以借助F来恢复出照相机参数，而F可以从对应的投影图像点计算出来。在不知道照相机的内参数（K₁和K₂）的情况下，仅能恢复照相机的投影变换矩阵。在知道照相机内参数的情况下，重建是基于度量的。所谓度量重建，即能够在三维重建中正确表示距离和角度。

利用上面的理论处理一些图像数据前，我们还需要了解一些几何学知识。给定图像中的一个点，例如第二个视图中的图像点x₂，利用上述公式，我们可以找到对应第一幅图像的一条直线：$$x_2^{T}F{x}_1=l_1^T{x}_1=0$$其中I₁^T=0是第一幅图像中的一条直线。这条线称为对应于点x₂的外极线，也就是说，点x₂在第一幅图像中的对应点一定在这条线上。所以，基础矩阵可以将对应点的搜索限制在外极线上。

外极线都经过一点e，称为外极点。实际上，外极点是另一个照相机光心对应的图像点。外极点可以在我们看到的图像外，这取决于照相机间的相对方向。因为外极点在所有的外极线上，所以Fe₁=0。因此，我们可以通过计算F的零向量来计算出外极点。同理，另一个外极点可以通过计算e₂^TF=0得到。

1.1 一个简单的数据集

我们需要一个带有图像点、三维点和照相机参数矩阵的数据集，在http://www.robots.ox.ac.uk/~vgg/data/mview/下载Merton College I

from pylab import *
from PCV.geometry import camera
from PIL import Image
# 载入一些图像
im1 = array(Image.open('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\001.jpg'))
im2 = array(Image.open('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\002.jpg'))

# 载入每个视图的二维点到列表中
points2D = [loadtxt('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\00'+str(i+1)+'.corners').T for i in range(3)]

# 载入三维点
points3D = loadtxt('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\p3d').T

# 载入对应
corr = genfromtxt('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\nview-corners')

# 载入照相机矩阵到 Camera 对象列表中
P = [camera.Camera(loadtxt('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\00'+str(i+1)+'.P')) for i in range(3)]

# 将三维点转换成齐次坐标表示，并投影
X = vstack( (points3D, ones(points3D.shape[1])))
x = P[0].project(X)

# 在视图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()

上面的程序会加载前两个图像（共三个）、三个视图中的所有图像特征点 1、对应不同视图图像点重建后的三维点以及照相机参数矩阵（使用上一章的 Camera 类）。这里使用 loadtxt() 函数读取文本文件到 NumPy 数组中。因为并不是所有的点都可见，或都能够成功匹配到所有的视图，所以对应数据里包含了缺失的数据。加载对应数据时需要考虑这一点。genfromtxt() 函数通过将缺失的数值（在文件中用 * 表示）填充为 -1 来解决这个问题。将三维的点投影到一个视图，然后和观测到的图像点比较，上面的代码绘制出第一个视图以及该视图中的图像点。为比较方便，投影后的点绘制在另一张图上。如果仔细观察，会发现第二幅图比第一幅图多一些点。这些多出的点是从视图 2 和视图 3 重建出来的，而不在视图 1 中。

1.2 用Matplotlib绘制三维数据

为了可视化三维重建结果，我们需要绘制出三维图像。Matplotlib中的mplot3d工具可以方便地绘制出三维点、线、等轮廓线、表面以及其他基本图形组件，还可以通过图像窗口控件实现三维旋转和缩放。

from mpl_toolkits.mplot3d import axes3d
from pylab import *

fig = figure()
ax = fig.gca(projection="3d")

# 生成三维样本点
X, Y, Z = axes3d.get_test_data(0.25)

# 在三维中绘制点
ax.plot(X.flatten(),Y.flatten(),Z.flatten(),'o')

show()

现在通过画出Merton样本数据来观察三维点的效果：

fig = figure()
ax = fig.gca(projection='3d')
ax.plot(points3D[0],points3D[1],points3D[2],'k.')
show()

俯视图，展示建筑墙体和屋顶上的点：

1.3 计算F：八点法

八点法是通过对应点来计算基础矩阵的算法。理论在：基本矩阵的基本解法之8点算法

八点算法的基本步骤：
1）求线性解 由系数矩阵A最小奇异值对应的奇异向量f求的F。
2）奇异性约束 是最小化Frobenius范数$\Vert F-F^{{}^{\prime }}\Vert$中的F^’代替F。

新建一个文件sfm.py，写入下面八点法中最小化$||Af||$的函数：

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

我们通常用SVD算法来计算最小二乘解。由于上面算法得出的解可能秩不为2（基础矩阵的秩小于等于2），所以我们通过将最后一个奇异值置0来得到秩最接近2的基础矩阵。这是个很有用的技巧。上面的函数忽略了一个重要的步骤：对图像坐标进行归一化，这可能会带来数值问题。

实验步骤：

1）sift提取特征
2）RANSAC去除错误点匹配
3）归一化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

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


# 载入图像，并计算特征
im1 = array(Image.open('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\book_frontal.JPG'))
sift.process_image('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\book_frontal.JPG','D:\\123\图像处理\Image Processing\Image Processing\Chapter5\im0.sift')
l1, d1 = sift.read_features_from_file('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\im0.sift')

im2 = array(Image.open('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\book_perspective.JPG'))
sift.process_image('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\book_perspective.JPG','D:\\123\图像处理\Image Processing\Image Processing\Chapter5\im1.sift')
l2, d2 = sift.read_features_from_file('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\im1.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 = x1.copy()
x2n = x2.copy()
print(len(ndx))

figure(figsize=(16,16))
sift.plot_matches(im1, im2, l1, l2, matches, True)
show()

# Don't use K1, and K2

#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 = 20 # 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
# 使用 RANSAC 方法估计 E
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]<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()

print(F)

x1e = []
x2e = []
ers = []
for i,m in enumerate(matches):
    if m>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]<cols1) and (0<= x2s[i][0]<cols1) and (0<=x1s[i][1]<rows1) and (0<=x2s[i][1]<rows1):
        plot([x1s[i][0], x2s[i][0]+cols1],[x1s[i][1], x2s[i][1]],'c')
axis('off')
show()

sift结果：

使用ransac去除错误点后：

基础矩阵：

[[-1.68832366e-07 -2.20529041e-07  1.13861932e-05]
[ 2.33866361e-07 -8.78615347e-07 -6.83017022e-04]
[-1.15489023e-03  1.27861930e-04  1.00000000e+00]]

1.4 外极线和外极点

本节开始提过，外极点满足Fe₁=0，因此可以通过计算F的零空间来得到。把下面的函数添加到sfm.py中：

def compute_epipole(F):
    """从基础矩阵F中计算右极点（可以使用F.T获得左极点）"""

    # 返回F的零空间（Fx=0）
    U,S,V = linalg.svd(F)
    e = V[-1]
    return e/e[2]

如果想获得另一幅图像的外极点（对应左零空间的外极点），只需将F FF转置后输入上述函数即可。

我们可以在之前样本数据集的前两个视图上运行这两个函数：

from pylab import *
from PCV.geometry import camera
from PIL import Image
from mpl_toolkits.mplot3d import axes3d
from PCV.geometry import sfm
# 载入一些图像
im1 = array(Image.open('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\001.jpg'))
im2 = array(Image.open('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\002.jpg'))

# 载入每个视图的二维点到列表中
points2D = [loadtxt('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\00'+str(i+1)+'.corners').T for i in range(3)]

# 载入三维点
points3D = loadtxt('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\p3d').T

# 载入对应
corr = genfromtxt('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\nview-corners',dtype='int',missing_values='*')

# 载入照相机矩阵到 Camera 对象列表中
P = [camera.Camera(loadtxt('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\00'+str(i+1)+'.P')) for i in range(3)]

# 将三维点转换成齐次坐标表示，并投影
X = vstack( (points3D, ones(points3D.shape[1])))
x = P[0].project(X)

# # 在视图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()

# fig = figure()
# ax = fig.gca(projection='3d')
# ax.plot(points3D[0],points3D[1],points3D[2],'k.')
# show()
#在前两个视图中点的索引
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为真，外极点会被画出来（如果输入参数中没有外极点，外极点会在程序中计算获得）。用不同的颜色将点和对应的外极线对应起来。

2 照相机和三维结构计算

上一节讲述了视图和基础矩阵、外极线计算方法的关系。这一节我们将简单地介绍计算照相机参数和三维结构的工具。

2.1 三角剖分

给定照相机参数模型，图像点可以通过三角剖分来恢复出这些点的三维位置。基本的算法思想如下。

对于两个照相机P₁和P₂的视图，三维实物点X的投影点为x₁和x₂（这里用齐次坐标表示），照相机方程定义了下列关系：$$\left[\begin{array}{ccc}{P}_1& -x_1& 0\\ P_2& 0& -x_2\end{array}\right]\left[\begin{array}{c}X\\ {\lambda }_1\\ {\lambda }_2\end{array}\right]=0$$由于图像噪声、照相机参数误差和其他系统误差，上面的方程可能没有精确解。我们可以通过SVD算法来得到三维点的最小二乘估值。

下面的函数用于计算一个点对的最小二乘三角剖分，把它添加到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]

最后一个特征向量的前4个值就是齐次坐标下的对应三维坐标。我们可以增加下面的函数来实现多个点的三角剖分：

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

我们可以利用下面的代码来实现Merton1数据集上的三角剖分：

# 获得前两个视图中点的索引
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()

上面的代码首先利用前两个视图的信息来对图像点进行三角剖分，然后把前三个图像点的齐次坐标输出到控制台，最后绘制出回复的最接近三维图像点。输出到控制台的信息如下：

[[ 1.03743725  1.56125273  1.40720017]
 [-0.57574987 -0.55504127 -0.46523952]
 [ 3.44173797  3.44249282  7.53176488]
 [ 1.          1.          1.        ]]
[[ 1.0378863   1.5606923   1.4071907 ]
 [-0.54627892 -0.5211711  -0.46371818]
 [ 3.4601538   3.4636809   7.5323397 ]
 [ 1.          1.          1.        ]]

算法估计出的三维图像点和实际图像点很接近，从结果绘图来看，估计点和实际点可以很好地匹配。

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

如果已经知道了一些三维点及其图像投影，我们可以使用直接线性变换的方法来计算照相机矩阵P。本质上，这是三角剖分方法的逆问题，有时我们将其称为照相机反切法。利用该方法恢复照相机矩阵同样也是一个最小二乘问题。

我们从照相机方程可以得出，每个三维点X_i（齐次坐标下）按照${\lambda }_i{x}_i=P{X}_i$投影到图像点$x_i=[x_i,y_i,1]$，相应的点满足下面的关系：
$$\left[\begin{array}{ccccccc}{X}_1^T& 0& 0& -x_1& 0& 0& ...\\ 0& X_1^T& 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& ...\\ ...& ...& ...& ...& ...& ...& ...\end{array}\right]\left[\begin{array}{c}{p}_1^T\\ p_2^T\\ p_3^T\\ {\lambda }_1\\ {\lambda }_2\\ ...\end{array}\right]=0$$其中p₁，p₂，p₃是矩阵P的三行。上面的式子可以写的更紧凑，如下所示：$$Mv=0$$然后，我们可以使用SVD分解估计出照相机矩阵。利用上面讲述的矩阵操作，我们可以直接写出相应的代码，加入到sfm.py中：

def compute_P(x, X):
    """由二维-三维对应对（齐次坐标表示）计算照相机矩阵"""
    
    n = x.shape[1]
    if X.shape[1] != n:
        raise ValueError("Number of points don't match")
    
    # 创建用于计算DLT解的矩阵
    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))

该函数的输入参数为图像点和三维点，构造出上述所示的M矩阵。最后一个特征向量的前12个元素是照相机矩阵的元素，经过重新排列成矩阵形状后返回。

下面，在我们的样本数据集上测试算法性能。下面的代码会选出第一个视图中的一些可见点（使用对应列表中缺失的数值），将它们转化为齐次坐标表示，然后估计照相机矩阵：

from pylab import *
from PIL import Image
from mpl_toolkits.mplot3d import axes3d
from PCV.geometry import camera
from PCV.geometry import sfm
from PCV.localdescriptors import sift
# 载入一些图像
im1 = array(Image.open('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\001.jpg'))
im2 = array(Image.open('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\002.jpg'))

# 载入每个视图的二维点到列表中
points2D = [loadtxt('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\00'+str(i+1)+'.corners').T for i in range(3)]

# 载入三维点
points3D = loadtxt('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\p3d').T

# 载入对应
corr = genfromtxt('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\nview-corners',dtype='int',missing_values='*')

# 载入照相机矩阵到 Camera 对象列表中
P = [camera.Camera(loadtxt('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\00'+str(i+1)+'.P')) for i in range(3)]

# 将三维点转换成齐次坐标表示，并投影
X = vstack( (points3D, ones(points3D.shape[1])))
x = P[0].project(X)

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()

为了检查照相机矩阵的正确性，将它们以归一化的格式（除以最后一个元素）打印到控制台。输出如下所示：

[[ 1.06528122e+00 -5.23431644e+01  2.06903697e+01  5.08729559e+02]
 [-5.05773247e+01 -1.33243075e+01 -1.47388581e+01  4.79179029e+02]
 [ 3.05145193e-03 -3.19263620e-02 -3.43702713e-02  1.00000000e+00]]
[[ 1.06774679e+00 -5.23448212e+01  2.06926980e+01  5.08764487e+02]
 [-5.05834364e+01 -1.33201976e+01 -1.47406641e+01  4.79228998e+02]
 [ 3.06792659e-03 -3.19008054e-02 -3.43665129e-02  1.00000000e+00]]

上面是估计出的照相机矩阵，下面是数据集的创建者计算出的照相机矩阵。可以看到，它们的元素几乎相同，最后使用估计出的照相机矩阵投影这些三维点，最后绘制出投影后的结果，结果如下图，真实点用圆圈表示，估计出的照相机投影点用点表示。

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

在两个场景中，照相机矩阵可以由基础矩阵恢复出来。假设第一个照相机矩阵归一化为P₁=[I|0]，现在我们需要计算出第二个照相机矩阵P₂。研究分为两类，未标定的情况和已标定的情况。

1.未标定的情况——投影重建
在没有任何照相机内参数知识的情况下，照相机矩阵只能通过射影变换恢复出来。也就是说，如果利用照相机的信息来重建三维点，那么该重建只能由射影变换计算出来（你可以得到整个投影场景中无畸变的重建点）。在这里，我们不考虑角度和距离。

因此，在无标定的情况下，第二个照相机矩阵可以使用一个（3×3）的射影变换得出。一个简单的方法是：$$P_2=[S_{e}F|e]$$其中，e是左极点，满足e^TF=0，S_e是反对称矩阵。请注意，使用该矩阵做出的三角剖分很有可能会发生畸变，如倾斜的重建。

下面两个函数添加到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]])

2.已标定的情况——度量重建
在已经标定的情况下，重建会保持欧式空间中的一些度量特性（除了全局的尺度参数）。在重建三维场景中，已标定的例子更为有趣。

给定标定矩阵K，我们可以将它的逆K^-1作用于图像点x_k=K^-1x，因此，在新的图像坐标系下，照相机方程变为：$$x_K=K^{-1}K[R|t]X=[R|t]X$$在新的图像坐标系下，点同样满足之前的基础矩阵方程：$$x_{K_2}^{T}F{x}_{K_1}=0$$在标定归一化的坐标系下，基础矩阵称为本质矩阵。为了区别为标定后的情况，以及归一化了的图像坐标，通常将其记为E，而非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

3 多视图重建

下面让我们来看，如何使用上面的理论从多幅图像中计算出真实的三维重建。由于照相机的运动给我们提供了三维结构，所以这样计算三维重建的方法通常称为SfM（Structure from Motion，从运动中恢复结构）。

假设照相机已经标定，计算重建可以分为下面4个步骤：
1）检测特征点，然后在两幅图像间匹配
2）由匹配计算基础矩阵
3）由基础矩阵计算照相机矩阵
4）三角剖分这些三维点

我们已经具备了完成上面4个步骤所需的所有工具。但是当图像间的点对应包含不正确的匹配时，我们需要一个稳健的方法来计算基础矩阵。

3.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

3.2 三维重建示例

from PIL import Image
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 mpl_toolkits.mplot3d import axes3d

# 标定矩阵
K = array([[2394, 0, 932], [0,2398,628], [0,0,1]])

# 载入图像
im1 = array(Image.open('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\alcatraz1.jpg'))
sift.process_image('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\alcatraz1.jpg','im2.sift')
l1, d1 = sift.read_features_from_file('im2.sift')

im2 = array(Image.open('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\alcatraz2.jpg'))
sift.process_image('D:\\123\图像处理\Image Processing\Image Processing\Chapter5\\alcatraz2.jpg','im3.sift')
l2, d2 = sift.read_features_from_file('im3.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)

# 计算照相机矩阵
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]

# 绘制三维图像
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.3 多视图的扩展示例

多视图重建有一些步骤和进一步的扩展，下面提供关于一些有关内容的介绍。

1）多视图
当我们有同一场景的多个视图时，三维重建会变得更加准确，包括更多的细节信息。因为基本矩阵只和一对视图相关，所以该过程带有很多图像，和之前的处理有些不同。
对于视频序列， 可是使用时序关系，在连续的帧对中匹配特征。相对方位需要从每个帧对增量地加入下一个帧对。同时可以使用跟踪有效地找到对应点。
对于静止的图像， 一个办法是找到一个中央参考视图，然后计算与之有关的所有其他照相机矩阵。另一个办法是计算一个图像对的照相机矩阵和三维重建，然后增量地加入新的图像和三维点。另外，还有一些方法可以同时由三个视图计算三维重建和照相机位置。

2）光束法平差
恢复出的点的位置和由估计的基础矩阵计算出的照相机矩阵都存在误差。在多个视图的计算中，这些误差会进一步累积。因此，多视图重建的最后一步，通常是通过优化三维点的位置和照相机参数来减少二次投影误差 。 该过程称为光束法平差。

3）自标定
在未标定照相机的情况中，有时可以从图像特征来计算标定矩阵。 该过程称为自标定。根据在照相机标定矩阵参数上做出的假设，以及可用的图像数据的类型（特征匹配、平行线、平面等），根据不同的自标定算法来进行标定。