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

    本章讲解如何处理多个视图，以及如何利用多个视图的几何关系来恢复照相机位置信息和三维结构。通过在不同视点拍摄的图像，我们可以利用特征匹配来计算出三维场景点以及照相机位置。

5.1外极几何

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

    两幅图像之间的约束关系使用代数的方式表示出来即为基础矩阵。基础矩阵可以用于简化匹配和去除错配特征。基础矩阵$F$满足：${xx}^{T}FFxx$=0

原理：

    设$X$在$C,C^{{}^{\prime }}$坐标系中的相对坐标分别为$p,p^{{}^{\prime }}$，则有$pp=RR{pp}^{{}^{\prime }}+TT$，其中$RR、TT$表示两个坐标系之间的旋转和位移关系。则根据三线共面，有$(pp-TT{)}^T(TT\times pp)=0$，可以推出$({RR}^T{pp}^{{}^{\prime }}{)}^T(TT\times pp)=0$
    令$TT\times pp=SSpp$，其中矩阵$SS$中的参数对应$TT$中的三个分量。$SS=\left[\begin{array}{ccc}0& -T_z& T_y\\ T_z& 0& -T_x\\ -T_y& T_x& 0\end{array}\right]$（叉积，秩为2）
    $\Rightarrow ({RR}^T{pp}^{{}^{\prime }}{)}^T(SSpp)=0$
    $\Rightarrow ({pp}^{{}^{\prime }T}RR)(SSpp)=0$
    $\Rightarrow {pp}^{{}^{\prime }T}EEpp=0$
$EE$为本质矩阵（essential matrix），描述了空间中的点在两个坐标系中的坐标对应关系。
    根据前述，$KK和{KK}^{{}^{\prime }}$分别为两个相机的内参矩阵，有：$pp={KK}^{-1}xx,{pp}^{{}^{\prime }}={KK}^{{}^{\prime }-1}{xx}^{{}^{\prime }}$
    $\Rightarrow ({KK}^{{}^{\prime }-1}{xx}^{{}^{\prime }}{)}^{T}EE({KK}^{-1}xx)=0$
    $\Rightarrow {xx}^{{}^{\prime }T}{KK}^{{}^{\prime }-T}EE{KK}^{-1}xx=0$
    $\Rightarrow {xx}^{{}^{\prime }T}FFxx=0$
$FF$即为基础矩阵（fundamental matrix），是外极几何的代数表达方式，描述了图像中任意点$x\iff x^{{}^{\prime }}$之间的约束关系。
    $FF$为$3\times 3$矩阵，秩为2，对任意匹配点对$x\iff x^{{}^{\prime }}$均满足${xx}^{T}FFxx$=0。$FF$有一些特殊的性质：
    1、转置：如果$FF$是表述点对$(x,x^{{}^{\prime }})$之间的基础矩阵，则${FF}^T$是表述点对$(x^{{}^{\prime }},x)$之间的基础矩阵；
    2、外极线：$FF$可以将点$x$映射到对应像平面上一条线$l=FF{xx}^{{}^{\prime }}$，同理可得$l^{{}^{\prime }}={FF}^{T}xx$；
    3、外极点：对于所有外极线，有${ee}^{T}FF{xx}^{{}^{\prime }}=0,\forall x^{{}^{\prime }}\Rightarrow e^{T}F=0$，同理有$FF{ee}^{{}^{\prime }}=0$；
    4、$FF$自由度为7（$3\times 3-1(homogeneous)-1(rank2)$）。

5.1.1一个简单的数据集

     我们需要一个带有图像点、三维点和找下那估计参数矩阵的数据集。这里使用一个牛津多视图数据集，从https://www.robots.ox.ac.uk/~vgg/data/mview/中下载MertonCollege1的数据。
代码：

from pylab import *
from PCV.geometry import camera
from PIL import Image
# 载入一些图像
im1 = array(Image.open('D:\\CV\MertonCollege1\\image\\001.jpg'))
im2 = array(Image.open('D:\\CV\MertonCollege1\\image\\002.jpg'))

# 载入每个视图的二维点到列表中
points2D = [loadtxt('D:\\CV\MertonCollege1\\2d\\00'+str(i+1)+'.corners').T for i in range(3)]

# 载入三维点
points3D = loadtxt('D:\\CV\MertonCollege1\\3d\p3d').T

# 载入对应
corr = genfromtxt('D:\\CV\MertonCollege1\\2d\\nview-corners')

# 载入照相机矩阵到 Camera 对象列表中
P = [camera.Camera(loadtxt('D:\\CV\MertonCollege1\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()
imshow(im1)
plot(points2D[0][0], points2D[0][1],'*')
axis('off')

figure()
imshow(im1)
plot(x[0],x[1],'r.')
axis('off')
show()

结果：

分析：
     左图为001.jpg和图像点，右图为001.jpg和投影的三维点。仔细观察发现，第二幅图比第一幅图多一些点，这些多出的点是从002.jpg和003.jpg重建出来的，而不在001.jpg中。例如结果中紫色方框的区域中的点。

5.1.2用Matplotlib绘制三维数据

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

在立体表面上画出三维点：

from mpl_toolkits.mplot3d import axes3d
from pylab import *

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

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

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

show()

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

from pylab import *

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

# 载入三维点
points3D = loadtxt('D:\\CV\MertonCollege1\\3d\p3d').T

ax.plot(points3D[0],points3D[1],points3D[2],'k.')
show()

初始显示的三维点图：

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

侧视图，展示了一面墙的轮廓，以及另一面墙上点的主视图：

分析：
     可以通过旋转上面的坐标系，在不同三维角度显示这些三维数据点的分布。

5.1.3计算$FF$：八点法

8点算法估算基础矩阵$FF$
    由于基础矩阵$FF$定义为${xx}^{{}^{\prime }T}FFxx=0$，任给两幅图像中的匹配点$x与x_{{}^{\prime }}$，令$x=(u,v,1{)}^T,x_{{}^{\prime }}=(u^{{}^{\prime }},v^{{}^{\prime }},1{)}^T，FF=\left[\begin{array}{ccc}{f}_{11}& f_{12}& f_{13}\\ f_{21}& f_{22}& f_{23}\\ f_{31}& f_{32}& f_{33}\end{array}\right]$，有相应方程：
    $u{u}^{\prime }{f}_{11}+u{v}^{\prime }{f}_{12}+u{f}_{13}+v{u}^{\prime }{f}_{21}+v{v}^{\prime }{f}_{22}+v{f}_{23}+u^{\prime }{f}_{31}+v^{\prime }{f}_{32}+f_{33}=0$
那么当多个点时，就可组成：
    $\left[\begin{array}{ccccccccc}{u}_1{u}_1^{\prime }& u_1{v}_1^{\prime }& u_1& v_1{u}_1^{\prime }& v_1{v}_1^{\prime }& v_1& u_1^{\prime }& v_1^{\prime }& 1\\ u_2{u}_2{}^{\prime }& u_2{v}_2^{\prime }& u_2& v_2{u}_2{}^{\prime }& v_2{v}_2^{\prime }& v_2& u_2^{\prime }& v_2^{\prime }& 1\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ u_n{u}_n{}^{\prime }& u_n{v}_n^{\prime }& u_n& v_n{u}_n^{\prime }& v_n{v}_n^{\prime }& v_n& u_n{}^{\prime }& v_n^{\prime }& 1\end{array}\right]\left[\begin{array}{c}{f}_{12}\\ f_{13}\\ f_{21}\\ f_{22}\\ f_{23}\\ f_{31}\\ f_{32}\\ f_{33}\end{array}\right]=0$
    可以看成是$AAff=0$。
    在实际计算中，可以直接用$A{A}^T$的分解来求解参数，也可以用非线性优化，通过搜索$f$使得$\Vert AAff\Vert$最小化，同时满足$\Vert ff\Vert =1$的约束。也可以通过$AA$的奇异值分解来计算$ff$。
    上述求解后的$FF$不一定能满足秩为2的约束，因此还要在$FF$基础上加以约束。通过SVD分解可以解决上述问题，令$FF=UU\Sigma {VV}^T$，则
    $\Sigma =\left[\begin{array}{ccc}{\sigma }_1& 0& 0\\ 0& {\sigma }_2& 0\\ 0& 0& {\sigma }_3\end{array}\right]，令{\Sigma }^{\prime }=\left[\begin{array}{ccc}{\sigma }_1& 0& 0\\ 0& {\sigma }_2& 0\\ 0& 0& 0\end{array}\right]$
最终的解为${FF}^{{}^{\prime }}=UU{\Sigma }^{{}^{\prime }}{VV}^T$

归一化8点算法：
     当矩阵各列的数据尺度差异太大时，最小二乘得到的结果精度一般很低。因此需要对各个列向量进行归一化操作：
    1、将图像坐标变换到合适的范围$\widehat{{xx}_i}=TT{xx}_i,{\widehat{{xx}_i}}^{{}^{\prime }}=TT{xx}_i^{{}^{\prime }}$；
    2、根据变换后的坐标$\widehat{{xx}_i},{\widehat{{xx}_i}}^{{}^{\prime }}$计算归一化基础矩阵$\widehat{FF}$；
    3、还原原始基础矩阵$FF={TT}^{{}^{\prime }T}\widehat{FF}TT$

实验步骤：
    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:\\CV\\python0.JPG'))
sift.process_image('D:\\CV\\python0.JPG','D:\\CV\\im0.sift')
l1, d1 = sift.read_features_from_file('D:\\CV\\im0.sift')

im2 = array(Image.open('D:\\CV\\python1.JPG'))
sift.process_image('D:\\CV\\python1.JPG','D:\\CV\im1.sift')
l2, d2 = sift.read_features_from_file('D:\\CV\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算法结果为：

使用ransac算法去除错误点后：

基础矩阵：

5.1.4外极点和外极线

     本章开头提到过，外极点满足$FF{ee}_1=0$，因此可以通过计算$FF$的零空间来得到。即如果想要获得另一幅图像的外极点（对应左零空间的外极点），只需将$FF$转置后输入到函数compute_epipole(F) 中。
代码：

from pylab import *
from PCV.geometry import camera
from PIL import Image
from PCV.geometry import sfm

# 载入一些图像
im1 = array(Image.open('D:\\CV\MertonCollege1\\image\\001.jpg'))
im2 = array(Image.open('D:\\CV\MertonCollege1\\image\\002.jpg'))

# 载入每个视图的二维点到列表中
points2D = [loadtxt('D:\\CV\MertonCollege1\\2d\\00'+str(i+1)+'.corners').T for i in range(3)]

# 载入三维点
points3D = loadtxt('D:\\CV\MertonCollege1\\3d\p3d').T

# 载入对应
corr = genfromtxt('D:\\CV\MertonCollege1\\2d\\nview-corners')

# 载入照相机矩阵到 Camera 对象列表中
P = [camera.Camera(loadtxt('D:\\CV\MertonCollege1\\2d\\00'+str(i+1)+'.P')) for i in range(3)]

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

#在前两个视图中点的索引
ndx = (corr[:,0]>=0) & (corr[:,1]>=0)

#获得坐标，并将其用齐次坐标表示
#x1 = points2D[0][:,corr[ndx,0]]
x1 = points2D[0][:,corr[ndx,0].astype('int64')]
x1 = vstack((x1,ones(x1.shape[1])))
#x2 = points2D[1][:,corr[ndx,1]]
x2 = points2D[1][:,corr[ndx,1].astype('int64')]
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为真，外极点会被画出来（如果输入参数中没有外极点，外极点会在程序中计算获得）。用不同的颜色将点和对应的外极线对应起来。
     实验过程中报错IndexError: arrays used as indices must be of integer (or boolean) type：

     这是一个索引错误：用作索引的数组必须是整数（或布尔）类型。只需要在代码中加上 .astype(‘int64’) 即可解决问题。

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

5.2.1三角剖分

     给定照相机参数模型，图像点可以通过三角剖分来恢复出这些点的三维位置。
     数学定义： 假设V是二维实数域上的有限点集，边e是由点集中的点作为端点构成的封闭线段, E为e的集合。那么该点集V的一个三角剖分T=(V,E)是一个平面图G，该平面图满足条件：
     1、除了端点，平面图中的边不包含点集中的任何点。
     2、没有相交边。
     3、平面图中所有的面都是三角面，且所有三角面的合集是散点集V的凸包。
     对于两个照相机$P_1和P_2$的视图，三维实物点$XX$的投影点为${xx}_1和{xx}_2$（用齐次坐标表示），照相机方程定义了下面的关系：
     $\left[\begin{array}{ccc}{P}_1& -{xx}_1& 0\\ P_2& 0& -{xx}_2\end{array}\right]\left[\begin{array}{c}XX\\ {\lambda }_1\\ {\lambda }_2\end{array}\right]=0$
     由于图像噪声、照相机参数误差和其他系统误差，上面的方程可能没有精确解。可以通过SVD算法来得到三维点的最小二乘估值。
     下面的函数用于计算一个点对的最小二乘三角剖分。第一个函数用于计算一个点对的最小二乘三角剖分，它的最后一个特征向量的前4个值就是齐次坐标系下的对应三维坐标。第二个函数用于实现多个点的三角剖分，这个函数的输入是两个图像点数组，输出为一个三维坐标数组：

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

下面通过代码来实现Merton1数据集上的三角剖分：
代码：

from pylab import *
from PCV.geometry import camera
from PCV.geometry import sfm

# 载入每个视图的二维点到列表中
points2D = [loadtxt('D:\\CV\MertonCollege1\\2d\\00'+str(i+1)+'.corners').T for i in range(3)]

# 载入三维点
points3D = loadtxt('D:\\CV\MertonCollege1\\3d\p3d').T

# 载入对应
corr = genfromtxt('D:\\CV\MertonCollege1\\2d\\nview-corners')

# 载入照相机矩阵到 Camera 对象列表中
P = [camera.Camera(loadtxt('D:\\CV\MertonCollege1\\2d\\00'+str(i+1)+'.P')) for i in range(3)]

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

# 在前两个视图中点的索引
ndx = (corr[:,0]>=0) & (corr[:,1]>=0)

#获得坐标，并将其用齐次坐标表示
#x1 = points2D[0][:,corr[ndx,0]]
x1 = points2D[0][:,corr[ndx,0].astype('int64')]
x1 = vstack((x1,ones(x1.shape[1])))
#x2 = points2D[1][:,corr[ndx,1]]
x2 = points2D[1][:,corr[ndx,1].astype('int64')]
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 = fig.add_subplot(projection='3d')
ax.plot(Xest[0],Xest[1],Xest[2],'ko')
ax.plot(Xtrue[0],Xtrue[1],Xtrue[2],'r.')
axis('equal')
show()

结果：


分析：
     上面的代码首先利用前两个视图的信息来对图像点进行三角剖分，然后把前三个图像点的齐次坐标输出到控制台，最后绘制出恢复的最接近三维图像点。从结果中可以看到，这个算法估计出的三维图像点和实际图像点很接近，估计点和实际点可以很好地进行匹配。

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

     如果已经知道了一些三维点及其图像投影，我们可以使用直接线性变换的方法来计算照相机矩阵$PP$。本质上，这是三角剖分方法的逆问题，有时将其称为照相机反切法。利用该方法恢复照相机矩阵同样也是一个最小二乘问题。
     从照相机方程可以得出，每个三维点${XX}_i$（齐次坐标系下）按照${\lambda }_i{xx}_i=PP{XX}_i$投影到图像点，相应的点满足下面的关系：
$$\left[\begin{array}{ccccccc}{\mathrm{X}}_1^{\mathrm{T}}& 0& 0& -{\mathrm{x}}_1& 0& 0& \dots \\ 0& {\mathrm{XT}}_1& 0& -{\mathrm{y}}_1& 0& 0& \dots \\ 0& 0& {\mathrm{X}}_1^{\mathrm{T}}& -1& 0& 0& \dots \\ {\mathrm{X}}_2^{\mathrm{T}}& 0& 0& 0& -{\mathrm{x}}_2& 0& \dots \\ 0& {\mathrm{X}}_2^{\mathrm{T}}& 0& 0& -{\mathrm{y}}_2& 0& \dots \\ 0& 0& {\mathrm{X}}_2^{\mathrm{T}}& 0& -1& 0& \dots \\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\end{array}\right]\left[\begin{array}{c}{\mathrm{p}}_1^{\mathrm{T}}\\ {\mathrm{p}}_2^{\mathrm{T}}\\ {\mathrm{p}}_3^{\mathrm{T}}\\ {\lambda }_1\\ {\lambda }_2\\ ⋮\end{array}\right]=0$$
其中${pp}_1,{pp}_2和{pp}_3$是矩阵$PP$的三行。上面的式子可以写成：$MMvv=0$，可以使用SVD分解估计出照相机矩阵。利用上面讲述的矩阵操作，直接编写实验代码。
     下面在之前使用的样本数据集上测试算法的性能。下面编写的代码会选出第一个视图中的可见点（使用对应列表中缺失的数值），将它们转换成齐次坐标表示，然后估计照相机矩阵。
代码：

from pylab import *
from PIL import Image
from PCV.geometry import camera
from PCV.geometry import sfm

# 载入一些图像
im1 = array(Image.open('D:\\CV\MertonCollege1\\image\\001.jpg'))
im2 = array(Image.open('D:\\CV\MertonCollege1\\image\\002.jpg'))

# 载入每个视图的二维点到列表中
points2D = [loadtxt('D:\\CV\MertonCollege1\\2d\\00'+str(i+1)+'.corners').T for i in range(3)]

# 载入三维点
points3D = loadtxt('D:\\CV\MertonCollege1\\3d\p3d').T

# 载入对应
corr = genfromtxt('D:\\CV\MertonCollege1\\2d\\nview-corners')

# 载入照相机矩阵到 Camera 对象列表中
P = [camera.Camera(loadtxt('D:\\CV\MertonCollege1\\2d\\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.astype('int64')] # 视图 1
x = vstack( (x,ones(x.shape[1])) )
X = points3D[:,ndx3D.astype('int64')]
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()

结果：


分析：
    第一个矩阵是估计出的照相机矩阵，第二个矩阵是数据集的创建者计算出的矩阵，可以看到它们的元素几乎完全相同。最后使用估计出的照相机矩阵投影这些三维点，然后绘制出投影后的结果。真实点用圆圈表示，估计出的照相机投影点用点表示。

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

    在两个视图场景中，照相机矩阵可以由基础矩阵恢复出来。假设第一个照相机矩阵归一化为${PP}_1=[I\mid 0]$，然后需要计算出第二个照相机矩阵${PP}_2$。分成两类情况，一类是未标定，另一类是已标定。
1、未标定的情况——投影重建
    在没有任何照相机内参数知识的情况下，照相机矩阵只能通过射影变换恢复出来。也就是说，如果利用照相机的信息来重建三维点，那么该重建只能由射影变换计算出来（你可以得到整个投影场景中无畸变的重建点）。在这里，我们不考虑角度和距离。
    因此，在无标定的情况下，第二个照相机矩阵可以使用一个$3\times 3$的射影变换得出。一个简单的方法是：${PP}_2=[{SS}_{e}FF\mid ee]$，其中$ee$是左极点，满足${ee}^{T}FF=0$，${SS}_e$是反对称矩阵。但是要注意的是使用该矩阵做出的三角形剖分很有可能会发生畸变，如倾斜的重建。
2、已标定的情况——度量重建
    在已经标定的情况下，重建会保持欧式空间中的一些度量特性（除了全局的尺度参数）。
    给定标定矩阵$KK$，可以将它的逆${KK}^{-1}$作用于图像点${xx}_K={KK}^{-1}xx$，因此，在新的图像坐标系下，照相机方程变为：${xx}_K={KK}^{-1}KK[RR\mid tt]XX=[RR\mid tt]XX$。在新的图像坐标系下，点同样满足之前的基础矩阵方程：${xx}_{K_2}^{T}FF{xx}_{K_2}=0$。在标定归一化的坐标系下，基础矩阵称为本质矩阵。为了区别为标定后的情况，以及归一化了的图像坐标，通常将其记为$EE$，而非$FF$。
    从本质矩阵中恢复出的照相机矩阵中存在度量关系，但有四个可能解。因为只有一个解产生位于两个照相机前的场景，所以可以从中选出来。

5.3多视图重建

    下面我们来实现从多幅图像中计算出真实的三维重建。
    由于照相机的运动给我们提供了三维结构，所以这样计算三维重建的方法称为SFM（Structure from Motion，从运动中恢复结构）。
    假设照相机已经标定，计算重建可以分为下面四个步骤：
    1、检测特征点，然后在两幅图像间匹配；
    2、由匹配计算基础矩阵；
    3、由基础矩阵计算照相机矩阵；
    4、三角剖分这些三维点。

    接下来使用由已知标定矩阵的照相机拍摄的两幅图像来观察一个简单的重建三维场景的完整例子。
代码：

# -*- coding: utf-8 -*-
from PIL import Image
from pylab import *
import Camera
from PCV.geometry import homography
from PCV.geometry import sfm
from PCV.localdescriptors import sift
# 标定矩阵
K = array([[2394,0,932],[0,2398,628],[0,0,1]])
# 载入图像，并计算特征


im1 = array(Image.open('D:\\CV\\EvilIsland0.jpg'))
sift.process_image('D:\\CV\\EvilIsland0.jpg', 'im0.sift')
l1, d1 = sift.read_features_from_file('im0.sift')

im2 = array(Image.open('D:\\CV\\EvilIsland1.jpg'))
sift.process_image('D:\\CV\\EvilIsland1.jpg', 'im1.sift')
l2, d2 = sift.read_features_from_file('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 = 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]

# 绘制三维图像
fig = figure()
#ax = fig.gca(projection='3d')
ax = fig.add_subplot(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算法算出的模型参数：

带有特征点和二次投影重建的三维点的两幅图像：

分析：
    代码通过循环遍历四个可能的解，每次都对对应于正确点的三维点进行三角剖分。将三角剖分后的图像投影回图像后，深度的符号由每个图像点的第三个数值给出。保存了正向深度的索引，对于和最优解一致的点，用相应的布尔量保存了信息，这样就可以取出真正在照相机前面的点。
    从实验结果中可以看到，二次投影后的点和原始特征位置不完全匹配， 但是相当接近（特征点为红色，二次投影重建的三维点为蓝色）。可以进一步调整照相机矩阵来提高重建和二次投影的性能。
    实验过程中报错：XXX takes 1 positional argument but 2 were given

    出现这个问题的原因是，函数在定义的时候少了self。但是我查看了PCV.geometry中的camera.py文件中函数在定义的时候并没有问题，之后尝试着导入在第四章中创建的Camera.py，然后就解决了这个问题。至于为什么，暂时还不明白。┭┮﹏┭┮