计算机视觉学习第5章——多视图几何

最新推荐文章于 2024-09-02 11:05:56 发布

jgq1466693

最新推荐文章于 2024-09-02 11:05:56 发布

阅读量4.7k

点赞数 4

文章标签：计算机视觉学习人工智能

本文链接：https://blog.youkuaiyun.com/jgq1466693/article/details/126139173

版权

一、外极几何

1.1 简单数据集

1.2 用Matplotlib绘制三维数据

一、外极几何

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

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

由于没有关于照相机的先验知识，会出现固有二义性，这是因为三维场景点X在经过4*4的单应性矩阵H变换后为HX，则HX在照相机 $PH^{-1}$ 里得到的图像点和X在照相机P里得到的图像点相同，利用照相机方程，可以描述为：

$\lambda x = PX = PH^{-1}HX = \hat{P}\hat{H}$

在分析双视图几何关系时，总是可以将照相机间的相对位置关系用单应性矩阵加以简化，这里的单应性矩阵通常只做刚体变换，一个比较好的做法是将原点和坐标轴与第一个照相机对齐：

$P_{1} = K_{1}[I|0]$ 和 $P_{2} = K_{2}[R|t]$

$K_{1},K_{2}$ 是标定矩阵，R是第二个照相机的旋转矩阵，t是第二个照相机的平移向量。利用这些照相机参数矩阵，可以找到点X的投影点 $x_{1},x_{2}$ 的关系。

同一个图像点经过不同的投影矩阵产生的不同投影点必须满足：

$x_{2}^{T}Fx_{1} = 0$

F为：

$F = K_{2}^{-T}S_{1}RK_{1}^{-1}$

$S_{1}$ 为反对称矩阵：

$S_{1} = \begin{bmatrix} 0 & -t_{3} & t_{2} \\ t_{3}& 0 & -t_{1} \\ -t_{2}& t_{1} & 0 \end{bmatrix}$

由上面的公式，就可以借助F来恢复出照相机参数，而F可以从对应的投影图像点计算出来。在不知道照相机内参数 $K_{1},K_{2}$ 的情况下，只能恢复照相机的投影变换矩阵。

度量重建就是能够在三维重建中正确的表示距离和角度。而在知道照相机内参数的情况下，重建就是基于度量的。

1.1 简单数据集

实验中使用的数据据是书中提供的牛津多视图数据集，可以从Visual Geometry Group - University of Oxford 中进行下载Merton数据集的压缩文件。解压后放入项目文件中即可。

实验代码：

from cam import Camera
from PIL import Image
from pylab import *
from numpy import *
# 载入图像
img1 = array(Image.open('D:\\picture\\images\\001.jpg'))
img2 = array(Image.open('D:\\picture\\images\\002.jpg'))

# 载入每个视图的二维点到列表中
points2D = [loadtxt('2D/00' + str(i+1) + '.corners').T for i in range(3)]
# 载入三维点
points3D = loadtxt('3D/p3d').T
# 载入对应
corr = genfromtxt('2D/nview-corners')
# 载入照相机矩阵到Camera对象列表中
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()
imshow(img1)
plot(points2D[0][0], points2D[0][1], '*')
axis('off')
figure()
imshow(img1)
plot(x[0], x[1], 'r.')
axis('off')
show()

在按照书中的代码进行操作时，出现了错误：

这里需要将

# 载入对应
corr = genfromtxt('2D/nview-corners', dtype='int', missing='*')

改成：

# 载入对应
corr = genfromtxt('2D/nview-corners')

这时运行就不会报错了，并能成功运行。

分析：上述代码主要功能是绘制出第一个视图以及该视图中的图像点，并将投影后的点绘制在另一张图上，我们可以看到第二幅图像中的点比第一幅要多一些，这些多出来的点来源于视图2和视图3的重建。

1.2 用Matplotlib绘制三维数据

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

from mpl_toolkits.mplot3d import axes3d
from PIL import Image
from pylab import *
from numpy 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()

get_teat_data()函数在x，y空间按照设定的空间间隔参数来产生三列数据点，接着将其输入plot()函数，这样就可以在立体表面上画出三维点。

还可以通过画出上一节中Merton数据集样本数据观察三维点的效果


from mpl_toolkits.mplot3d import axes3d
from PIL import Image
from pylab import *
from numpy import *
from cam import Camera
# 载入图像
img1 = array(Image.open('D:\\picture\\images\\001.jpg'))
img2 = array(Image.open('D:\\picture\\images\\002.jpg'))

# 载入每个视图的二维点到列表中
points2D = [loadtxt('2D/00' + str(i+1) + '.corners').T for i in range(3)]
# 载入三维点
points3D = loadtxt('3D/p3d').T
# 载入对应
corr = genfromtxt('2D/nview-corners')


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

得到了以下的视图。

1.3 计算F：八点法

八点法是通过计算对应点来计算基础矩阵的算法。

我们可以用线性系统的形式描述外极约束：

f包含F的元素， $x_{1}^{i} = [x_{1}^{i},y_{1}^{i},w_{1}^{i}],x_{2}^{i} = [x_{2}^{i},y_{2}^{i},w_{2}^{i}]$ 是一对图像点，共有n对对应点。基础矩阵中有9个元素，因其尺度是任意的，故只需8个方程，用8个对应点计算基础矩阵F，就将其称为八点法。

用python表示八点法中最小化 $\left | \left | Af \right | \right |$ 的函数：

def compute_fundamental(x1,x2):
    """ 使用归一化的八点算法，从对应点（x1,x2,x3...）中计算基础矩阵，每一行由如下构成
        [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/F[2,2]

SVD算法用于计算最小二乘解，因为上述算法的解可能到的秩并不为2，因此我们就通过将最后一个奇异值置0来得到秩最接近2的基础矩阵。当然这里还缺少一步对图像坐标的归一化，接下来的学习中会进一步讨论。

1.4 外极点和外极线

外极点满足条件 $Fe_{1}=0$ ，故可以从计算F的零空间得到，具体算法如下：

def compute_epipole(F):
    """ Computes the (right) epipole from a 
        fundamental matrix F. 
        (Use with F.T for left epipole.) """
    
    # return null space of F (Fx=0)
    U,S,V = linalg.svd(F)
    e = V[-1]
    return e/e[2]

接下来就是利用之前的样本数据集的前两个视图进行实验：

from PIL import Image
from pylab import *
from numpy import *
from PCV.geometry import sfm
# 载入图像
img1 = array(Image.open('D:\\picture\\images\\001.jpg'))
img2 = array(Image.open('D:\\picture\\images\\002.jpg'))

# 载入每个视图的二维点到列表中
points2D = [loadtxt('2D/00' + str(i+1) + '.corners').T for i in range(3)]
# 载入三维点
points3D = loadtxt('3D/p3d').T
# 载入对应
corr = genfromtxt('2D/nview-corners', dtype='int')
# 载入照相机矩阵到Camera对象列表中

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(img1)
# 分别绘制每条线
for i in range(5):
    sfm.plot_epipolar_line(img1, F, x2[:, i], e, False)
axis('off')
figure()
imshow(img2)
# 分别绘制每个点
for i in range(5):
    plot(x2[0, i], x2[1, i], 'o')
axis('off')
show()

选择两幅图像的对应点，并将其转换为齐次坐标，对应点是从文本文件中读取到的。代码中用到了plot_epipolar_line()函数，用于绘制外极点和外极线：

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*')

将x轴的范围作为直线的参数，当直线超出图像边界时，超出的部分就会被截断。当show_epipole为真时，外极点也会被化出来。

二、照相机和三维结构的计算

2.1 三角部分

通过给定照相机参数模型，图像点就可以通过三角剖分来恢复出这些点的三维位置。对于两个照相机 $P_{1},P_{2}$ 的视图，三维实物点X的投影点为 $x_{1},x_{2}$ ，照相机方程就为:

由于图像噪声、照相机的参数误差和其他系统误差，可能导致方程无精确解，因此可以通过SVD算法得到三维点的最小二乘估值。下面就给出计算一个点对的最小二乘三角剖分的python算法：

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中点的二视图的三角剖分 """
        
    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

三角剖分的实验：

from PCV.geometry import sfm
from PIL import Image
from pylab import *
from numpy import *
from cam import Camera
from mpl_toolkits.mplot3d import axes3d
# 载入图像
img1 = array(Image.open('D:\\picture\\images\\001.jpg'))
img2 = array(Image.open('D:\\picture\\images\\002.jpg'))

# 载入每个视图的二维点到列表中
points2D = [loadtxt('2D/00' + str(i+1) + '.corners').T for i in range(3)]
# 载入三维点
points3D = loadtxt('3D/p3d').T
# 载入对应
corr = genfromtxt('2D/nview-corners', dtype='int')
# 载入照相机矩阵到Camera对象列表中
P = [Camera(loadtxt('2D/00' + str(i+1) + '.P')) for i in range(3)]
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])))

Xture = points3D[:,ndx]
Xture = vstack((Xture,ones(Xture.shape[1])))
# 检查前三个点
Xest = sfm.triangulate(x1,x2,P[0].P,P[1].P)
print(Xest[:,:3])
print(Xture[:,:3])
# 绘制图像
fig = figure()
ax = fig.gca(projection='3d')
ax.plot(Xest[0],Xest[1],Xest[2],'ko')
ax.plot(Xture[0],Xture[1],Xture[2],'r.')
axis('auto')
show()

控制台得出结果，可以看出算法估计的三维图像点与实际图像点很接近。

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

直接线性变换的方法可以计算照相机矩阵P，本质上，这是三角剖分方法的逆问题，我们也将其称为照相机反切法。同样可以利用这个方法恢复照相机矩阵。

通过前面的学习已经得知，三维点 $X_{i}$ 按照 $\lambda _{i}x_{i} = PX_{i}$ ,投影到点 $x_{i} = [x_{i},y_{i},1]$ ，则相应点满足的关系是：

$p_{1},p_{2},p_{3}$ 是矩阵P的三行，用python代码可以表示为：

def compute_P(x,X):
    """ 由二维三维对对应计算照相机矩阵 """

    n = x.shape[1]
    if X.shape[1] != n:
        raise ValueError("Number of points don't match.")
        
    # create matrix for DLT solution
    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 cam import Camera
from PIL import Image
from pylab import *
from numpy import *
from PCV.geometry import sfm


img1 = array(Image.open('D:\\picture\\images\\001.jpg'))
img2 = array(Image.open('D:\\picture\\images\\002.jpg'))

# 载入每个视图的二维点到列表中
points2D = [loadtxt('2D/00' + str(i+1) + '.corners').T for i in range(3)]
# 载入三维点
points3D = loadtxt('3D/p3d').T
# 载入对应
corr = genfromtxt('2D/nview-corners', dtype='int')
# 载入照相机矩阵到Camera对象列表中
P = [Camera(loadtxt('2D/00' + str(i+1) + '.P')) for i in range(3)]
corr = corr[:, 0]  # 视图1
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])))
# 估计P
Pest = 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(img1)
plot(x[0],x[1],'bo')
plot(xest[0],xest[1],'r.')
axis('off')
show()

将归一化格式打印控制台：

观察两组数据可以看出，估计得到的照相机矩阵与计算得出的照相机矩阵几乎一致。

使用估计得出的照相机矩阵投影三维点，绘制出结果：

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

研究分为两类：未标定的情况和已标定的情况

1、未被标定的情况——投影重建

当没有照相机内参数知识的时候，照相机矩阵就只能通过射影变换恢复出来了，一个简单的方法就是：

$P_{2} = [S_{e}F|e]$

e为左极点，满足 $e_{T}F = 0$ 。不过引用该矩阵时，三角形剖分很有可能会发生畸变，类似倾斜的重建之类的。

python代码为：

def compute_P_from_fundamental(F):
    """ 从基础矩阵中计算第二个照相机矩阵（假设P1 = [I 0]） """
        
    e = compute_epipole(F.T) # left epipole
    Te = skew(e)
    return vstack((dot(Te,F.T).T,e)).T

使用到的skew()函数定义为：

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^{-1}x$ ，在新的图像坐标系下，方程变为

$x_{K} = K^{-1}K[R|t]X = [R|t]X$

当然点同样也满足之前的基础方程：

$x_{K_{2}}^{T}Fx_{K_{1}} = 0$

标定归一化的坐标系下，基础矩阵又称为本质矩阵，一般将标定后的矩阵记为E。

从本质矩阵恢复出来的照相机矩阵中存在度量关系，但有四个可能的解，且只有一个解产生位于两个照相机前的场景。

计算这四个解的算法为：

def compute_P_from_essential(E):
    """ 从本质矩阵中计算第二个照相机的矩阵，输出为可能的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))    
    
    # 创建矩阵
    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

函数要确保本质矩阵的秩为2。

三、多视图重建

利用上述理论从多福图像中计算出真实的三维重建，假设照相机已经标定，计算重建可以分为4个步骤：

1、检测特征点，然后于两幅图像中进行匹配；

2、根据匹配计算基础矩阵

3、由基础矩阵计算照相机矩阵；

4、三角剖分三维点

3.1 稳健估计基础矩阵

当存在噪声和不正确的匹配时，需要估计基础矩阵，这里使用RANSAC方法，并结合八点算法。需要注意的是八点算法在平面场景就会失效，因此当场景点在平面上是该算法不能使用。

class RansacModel(object):
    """ 可用先前章节中学习的ransac算法计算基础矩阵的类 """
    
    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

使用fit()和get_error()方法，采用的错误衡量方法是Sampson距离。fit()方法会选择8个点，然后使用归一化的八点算法：

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]

上述函数将图像点归一化为零均值固定方差。接下来就是要知道哪些匹配和F矩阵是一致的了，这里就将上面的类使用在函数中：

def F_from_ransac(x1,x2,model,maxiter=5000,match_theshold=1e-6):
    """ 使用RANSAC方法从点对应中稳健地估计基础矩阵F，输入：使用齐次坐标表示的点x1，x2 """

    import ransac02

    data = vstack((x1,x2))
    
    # 计算F，并返回正确索引点
    F,ransac_data = ransac02.ransac(data.T,model,8,maxiter,match_theshold,20,return_all=True)
    return F, ransac_data['inliers']

返回最佳基础矩阵F，以及正确的索引。

3.2 三维重建

进行三维重建时，首先需要先进行特征的提取与匹配，之后进行基础矩阵和照相机矩阵的估计，在获得标定矩阵后对矩阵K进行硬编码，在挑选属于匹配关系的特定点后使用 $K^{-1}$ 进行归一化，并使用归一化的八点算法进行RANSAC估计，得到本质矩阵，从本质矩阵除法计算出第二个照相机矩阵的四个可能解。

from pylab import *
from numpy import *
from PIL import Image
from cam import Camera
from PCV.geometry import sfm, homography
from PCV.localdescriptors import sift
from mpl_toolkits.mplot3d import axes3d


# 标定矩阵
K = array([[2394,0,932],[0,2398,628],[0,0,1]])
# 载入图像，并计算特征
img1 = array(Image.open('D:\\picture\\images\\001.jpg'))
sift.process_image('D:\\picture\\images\\001.jpg', 'img1.sift')
l1,d1 = sift.read_features_from_file('img1.sift')
img2 = array(Image.open('D:\\picture\\images\\002.jpg'))
sift.process_image('D:\\picture\\images\\002.jpg', 'img2.sift')
l2,d2 = sift.read_features_from_file('img2.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)

对4个解进行循环遍历，对对应于正确点的三维点进行三角剖分，并将三角剖分后的图像投影回图像后，深度的符号由每个图像点的第三个数值给出。

# 选取点在照相机前的解
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')
# 绘制三维点
cam1 = Camera(P1)
cam2 = Camera(P2[ind])
x1p = cam1.project(X)
x2p = cam2.project(X)
x1p = dot(K,x1p)
x2p = dot(K,x2p)
figure()

imshow(img1)
gray()
plot(x1p[0],x1p[1],'o')
plot(x1[0],x1[1],'r.')
axis('off')
figure()

imshow(img2)
gray()
plot(x2p[0],x2p[1],'o')
plot(x2[0],x2[1],'r.')
axis('off')
show()