(01)ORB-SLAM2源码无死角解析-(37) EPnP 算法原理详解→理论基础一:控制点选取、透视投影约束

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下:
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196

文末正下方中心提供了本人联系方式，点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证

一、背景介绍

论文： Lepetit V , Fua M N . EPnP: An Accurate O(n) Solution to the PnP Problem[J]. International Journal of
Computer Vision, 2009.

该算法功能如下：

输入：1.n个世界坐标的下的3D点，论文中称为3D参考点。2.这n个3D点投影在图像上的2D坐标3. 相机内参矩阵 ，包括焦距和主点输出: 相机的位姿R,t应用：特征点的图像跟踪，需要实时处理有噪声的特征点，对计算精度和效率要求比较高，只需4对匹配点即可求解。

算法的优点：

 1.只需要4对非共面点，对于平面只需要3对点2.闭式解，不需要迭代，不需要初始估计值。3.精度比较高。和迭代法里精度最高的方法LHM方法精度相当。4.比较鲁棒，可以处理带噪声的数据。迭代法受到初始估计的影响比较大，会不稳定5.线性计算复杂度为O(n)6.平面和非平面都适用

二、原理和步骤：

目的 : \color{blue}{目的}: 目的: n个世界坐标系下的3D点及其在图像上的2D投影点，还有相机还有相机内参，目的是为了求世界坐标系到相机坐标系下的位姿变换 R , t \mathbf R,\mathbf t R,t 。

EPnP的思路就是先把2D图像点通过内参变换到相机坐标系下的3D点，然后用ICP来求解3D-3D的变换就得到了位姿。那么问题的核心就转化为如何通过2D信息，加上一些约束，来得到相机坐标系下的3D点。因为我们这里的位姿变换是欧式空间下的刚体变换，所以点之间的相对距离信息在不同坐标系下是不变的。我们称之为刚体结构不变性。后面就是紧紧围绕这个特性来求解的

( 1 ) : \color{blue}{(1)}: (1):首先我们对3D点的表达方式进行了新的定义。之前不管是世界坐标系还是相机坐标系下的3D点，它们都是相对于自己坐标系下的原点的。那么两个坐标系原点不同，坐标的量级可能差异非常大，比如相机坐标系下3D点坐标范围可能是10-100之间，世界坐标系下坐标可能是1000-10000之间，这对求解优化都是不利的。所以我们要统一一下量级。可以理解为归一化吧，这在求基础矩阵、单应矩阵时都是常规手段。
具体来说，我们对每个坐标系定义4个控制点，其中一个是质心（也就是各个方向均值），其他3个用PCA从三个主方向选取，这4个控制点可以认为是参考基准，类似于坐标系里的基。所有的3D点都表达为这4个参考点的线性组合。这些系数我们称之为权重，为了不改变数据的相对距离，权重和必须为1。这样，我们就可以用世界坐标系或相机坐标系下的4个控制点表示所有的世界坐标系或相近坐标系下的3D点。

( 2 ) : \color{blue}{(2)}: (2):利用投影方程将图像2D点恢复相机坐标系下3D点（未知量）。经过整理后，一组点对可以得到2个方程。我们待求的相机坐标系下3D点对应的4个控制点，每个控制点3个分量，总共12个未知数组成的一个向量。

( 3 ) : \color{blue}{(3)}: (3):用SVD分解可以求解上述向量，但是因为恢复的相机坐标系下3D点还有个尺度因子 β \beta β，这里我们根据结构信息不变性作为约束，求解。

( 4 ) : \color{blue}{(4)}: (4):最后用高斯牛顿法优化上述求解的 β \beta β 。

注：ORB-SLAM里使用的EPnP是直接拷贝OpenCV里的源码：modules → calib3d → src → epnp.cpp

三、统一变量格式：

为了方便理解后续的公式推导，首先统一一下变量的定义格式：
( 1 ) : \color{blue}{(1)}: (1): 用上标 w ^w w 和 c ^c c 分别表示世界坐标系和相机坐标系中的坐标。

( 2 ) : \color{blue}{(2)}: (2): n个3D参考点在世界坐标系中的坐标是已知输入 \color{red}{已知输入} 已知输入，记为 p i w , i = 1 , ⋯ , n \mathbf p_i^w, i=1,\cdots,n piw,i=1,⋯,n。

( 3 ) : \color{blue}{(3)}: (3): n个3D参考点在相机坐标系下的坐标是未知的 \color{red}{未知的} 未知的，记为 p i c , i = 1 , ⋯ , n \mathbf p_i^c, i=1,\cdots,n pic,i=1,⋯,n。

( 4 ) : \color{blue}{(4)}: (4): n个3D参考点在相机坐标系下对应的n个2D投影坐标是已知输入 \color{red}{已知输入} 已知输入，记为 u i , i = 1 , ⋯ , n \mathbf u_i, i=1,\cdots,n ui,i=1,⋯,n。

( 5 ) : \color{blue}{(5)}: (5): 4个控制点在世界坐标系的坐标为 c j w , j = 1 , ⋯ , 4 \mathbf c_j^w, j=1,\cdots,4 cjw,j=1,⋯,4

( 6 ) : \color{blue}{(6)}: (6): 4个控制点在世界坐标系的是未知的 \color{red}{未知的} 未知的，记为 c j c , j = 1 , ⋯ , 4 \mathbf c_j^c, j=1,\cdots,4 cjc,j=1,⋯,4

( 7 ) : \color{blue}{(7)}: (7): 4个控制点系数 α i j , i = 1 , ⋯ , n \alpha_{ij}, i=1,\cdots,n αij,i=1,⋯,n， j = 1 , ⋯ , 4 j=1,\cdots,4 j=1,⋯,4,也就是也就是论文中的homogeneous barycentriccoordinates，我们翻译为齐次重心坐标。

注意: 同一3D点在世界坐标系下和相机坐标系下的控制点系数相同。后面会给出证明。另外以上坐标都是非齐次坐标，后面也都是非齐次坐标。

四、控制点如何选取

理论上，控制点的坐标可以任意选取。但在实践中，作者发现了一种可以提高结果稳定性的控制点选择
方法。具体如下
( 1 ) : \color{blue}{(1)}: (1):将参考点的质心（或者称为重心、均值中心）设置为其中一个控制点，表达式如下。这是有一定物理意义的，因为后续会使用质心对坐标点进行归一化。 c 1 w = 1 n ∑ i = 1 n p i w (01) \color{Green} \tag{01} \mathbf{c}_{1}^{w}=\frac{1}{n} \sum_{i=1}^{n} \mathbf{p}_{i}^{w} c1w=n1i=1∑npiw(01) ( 2 ) : \color{blue}{(2)}: (2):剩下的3个控制点从数据的三个主方向上选取。我们对世界坐标系下3D点集合 p i w , i = 1 , ⋯ , n {\mathbf p_i^w, i=1,\cdots,n} piw,i=1,⋯,n，去质心后得到 A = [ ( p 1 w ) T − ( c 1 w ) T ⋮ ( p n w ) T − ( c 1 w ) T ] (02) \color{Green} \tag{02} \mathbf{A}=\left[\begin{array}{c} \left(\mathbf{p}_{1}^{w}\right)^{T}-\left(\mathbf{c}_{1}^{w}\right)^{T} \\ \vdots \\ \left(\mathbf{p}_{n}^{w}\right)^{T}-\left(\mathbf{c}_{1}^{w}\right)^{T} \end{array}\right] A= (p1w)T−(c1w)T⋮(pnw)T−(c1w)T (02) A \mathbf A A是一个 nx3 的矩阵，那么 A T A \mathbf A^T\mathbf A ATA 就是 3x3 的方阵，通过对矩阵 A T A \mathbf A^T\mathbf A ATA 进行特征值分解，得到三个特征值 λ 1 w , λ 2 w , λ 3 w \lambda_{1}^{w}, \lambda_{2}^{w},\lambda_{3}^{w} λ1w,λ2w,λ3w, 他们对应的特征向量为 v 1 w , v 2 w , v 3 w v_{1}^{w}, v_{2}^{w}, v_{3}^{w} v1w,v2w,v3w。将剩余的3个控制点表示为 c 2 w = c 1 w + λ 1 w n v 1 w c 3 w = c 1 w + λ 2 w n v 2 w c 4 w = c 1 w + λ 3 w n v 3 w (03) \color{Green} \tag{03} \begin{array}{l} \mathbf{c}_{2}^{w}=\mathbf{c}_{1}^{w}+\sqrt{\frac{\lambda_{1}^{w}}{n}} v_{1}^{w} \\ \mathbf{c}_{3}^{w}=\mathbf{c}_{1}^{w}+\sqrt{\frac{\lambda_{2}^{w}}{n}} v_{2}^{w} \\ \mathbf{c}_{4}^{w}=\mathbf{c}_{1}^{w}+\sqrt{\frac{\lambda_{3}^{w}}{n}} v_{3}^{w} \end{array} c2w=c1w+nλ1w v1wc3w=c1w+nλ2w v2wc4w=c1w+nλ3w v3w(03)为什么要加上 c 1 w c_1^w c1w, 因为前面去了质心，所以要重新加上。这样就可以获得4在世界坐标系的控制点 c j w , j = 1 , ⋯ , 4 \mathbf c_j^w, j=1,\cdots,4 cjw,j=1,⋯,4。

五、计算控制点系数

我们将世界坐标系下3D点的坐标表示为对应控制点坐标的线性组合： p i w = ∑ j = 1 4 α i j c j w , ∑ j = 1 4 α i j = 1 (04) \color{Green} \tag{04} \mathbf{p}_{i}^{w}=\sum_{j=1}^{4} \alpha_{i j} \mathbf{c}_{j}^{w}, ~~~~\sum_{j=1}^{4} \alpha_{i j}=1 piw=j=1∑4αijcjw, j=1∑4αij=1(04)在论文中， α i j \alpha_{ij} αij 称为homogeneous barycentric coordinates，我们翻译为齐次重心坐标，它实际上表达的是世界坐标系下3D点在控制点坐标系下的坐标系数。当控制点 c j w c_j^w cjw 通过第一步的方法确定后， α i j \alpha_{ij} αij也是唯一确定的，推导如下： p i w = α i 1 c 1 w + α i 2 c 2 w + α i 3 c 3 w + α i 4 c 4 w (05) \color{Green} \tag{05} \mathbf{p}_{i}^{w}=\alpha_{i 1} \mathbf{c}_{1}^{w}+\alpha_{i 2} \mathbf{c}_{2}^{w}+\alpha_{i 3} \mathbf{c}_{3}^{w}+\alpha_{i 4} \mathbf{c}_{4}^{w} piw=αi1c1w+αi2c2w+αi3c3w+αi4c4w(05)3D点的重心为，也是围 c 1 w c_1^w c1w，也是第一个控制点，上式左右分别减去重心： p i w − c 1 w = α i 1 c 1 w + α i 2 c 2 w + α i 3 c 3 w + α i 4 c 4 w − c 1 w = α i 1 c 1 w + α i 2 c 2 w + α i 3 c 3 w + α i 4 c 4 w − ( α i 1 + α i 2 + α i 3 + α i 4 ) c 1 w = α i 2 ( c 2 w − c 1 w ) + α i 3 ( c 3 w − c 1 w ) + α i 4 ( c 4 w − c 1 w ) = [ c 2 w − c 1 w c 3 w − c 1 w c 4 w − c 1 w ] [ α i 2 α i 3 α i 4 ] (06) \color{Green} \tag{06} \begin{aligned} \mathbf{p}_{i}^{w}-\mathbf{c}_{1}^{w} &=\alpha_{i 1} \mathbf{c}_{1}^{w}+\alpha_{i 2} \mathbf{c}_{2}^{w}+\alpha_{i 3} \mathbf{c}_{3}^{w}+\alpha_{i 4} \mathbf{c}_{4}^{w}-\mathbf{c}_{1}^{w} \\ &=\alpha_{i 1} \mathbf{c}_{1}^{w}+\alpha_{i 2} \mathbf{c}_{2}^{w}+\alpha_{i 3} \mathbf{c}_{3}^{w}+\alpha_{i 4} \mathbf{c}_{4}^{w}-\left(\alpha_{i 1}+\alpha_{i 2}+\alpha_{i 3}+\alpha_{i 4}\right) \mathbf{c}_{1}^{w} \\ &=\alpha_{i 2}\left(\mathbf{c}_{2}^{w}-\mathbf{c}_{1}^{w}\right)+\alpha_{i 3}\left(\mathbf{c}_{3}^{w}-\mathbf{c}_{1}^{w}\right)+\alpha_{i 4}\left(\mathbf{c}_{4}^{w}-\mathbf{c}_{1}^{w}\right) \\ &=\left[\begin{array}{lll} \mathbf{c}_{2}^{w}-\mathbf{c}_{1}^{w} & \mathbf{c}_{3}^{w}-\mathbf{c}_{1}^{w} & \mathbf{c}_{4}^{w}-\mathbf{c}_{1}^{w} \end{array}\right]\left[\begin{array}{l} \alpha_{i 2} \\ \alpha_{i 3} \\ \alpha_{i 4} \end{array}\right] \end{aligned} piw−c1w=αi1c1w+αi2c2w+αi3c3w+αi4c4w−c1w=αi1c1w+αi2c2w+αi3c3w+αi4c4w−(αi1+αi2+αi3+αi4)c1w=αi2(c2w−c1w)+αi3(c3w−c1w)+αi4(c4w−c1w)=[c2w−c1wc3w−c1wc4w−c1w] αi2αi3αi4 (06)那么,世界坐标系下控制点的系数就可以这样计算得到： [ α i 2 α i 3 α i 4 ] = [ c 2 w − c 1 w c 3 w − c 1 w c 4 w − c 1 w ] − 1 ( p i w − c 1 w ) α i 1 = 1 − α i 2 − α i 3 − α i 4 (07) \color{Green} \tag{07} \begin{array}{c} {\left[\begin{array}{l} \alpha_{i 2} \\ \alpha_{i 3} \\ \alpha_{i 4} \end{array}\right]=\left[\begin{array}{ccc} \mathbf{c}_{2}^{w}-\mathbf{c}_{1}^{w} & \mathbf{c}_{3}^{w}-\mathbf{c}_{1}^{w} & \mathbf{c}_{4}^{w}-\mathbf{c}_{1}^{w} \end{array}\right]^{-1}\left(\mathbf{p}_{i}^{w}-\mathbf{c}_{1}^{w}\right)} \\ \alpha_{i 1}=1-\alpha_{i 2}-\alpha_{i 3}-\alpha_{i 4} \end{array} αi2αi3αi4 =[c2w−c1wc3w−c1wc4w−c1w]−1(piw−c1w)αi1=1−αi2−αi3−αi4(07)以上是世界坐标系下的推导，那么，在相机坐标系下， α i j \alpha_{ij} αij 满足如下的对应关系：
p i c = ∑ j = 1 4 α i j c j c , ∑ j = 1 4 α i j = 1 (08) \color{Green} \tag{08} \mathbf{p}_{i}^{c}=\sum_{j=1}^{4} \alpha_{i j} \mathbf{c}_{j}^{c},~~~~\sum_{j=1}^{4} \alpha_{i j}=1 pic=j=1∑4αijcjc, j=1∑4αij=1(08)前面已经提到世界坐标系满足(04)式的关系，那么相同的 α i j \alpha_{i j} αij 使得相机坐标系满足(08)式，这里我们给出一个结论 \color{red}{结论} 结论：同一个3D点在世界坐标系下对应控制点的系数 α i j \alpha_{i j} αij 和其在相机坐标系下对应控制点的系数相同。也就是说，我们可以预先在世界坐标系下求取控制点系数 α i j \alpha_{i j} αij，然后将其作为已知量拿到相机坐标系下使用。下面是该结论的推导过程( T c w 表示为需要求解的相机位姿 \mathbf T_{cw} 表示为需要求解的相机位姿 Tcw表示为需要求解的相机位姿)：
[ p i c 1 ] = T c w [ p i w 1 ] = T c w [ ∑ j = 1 4 α i j c j w ∑ j = 1 4 α i j ] = ∑ j = 1 4 α i j T c w [ c j w 1 ] = ∑ j = 1 4 α i j [ c j c 1 ] (09) \color{Green} \tag{09} \begin{aligned} {\left[\begin{array}{c} \mathbf{p}_{i}^{c} \\ 1 \end{array}\right] } &=\mathbf T_{cw} \left[\begin{array}{c} \mathbf{p}_{i}^{w} \\ 1 \end{array}\right] \\ &=\mathbf T_{cw} \left[\begin{array}{c} \sum_{j=1}^{4} \alpha_{i j} \mathbf{c}_{j}^{w} \\ \\ \sum_{j=1}^{4} \alpha_{i j} \end{array}\right] \\ &=\sum_{j=1}^{4} \alpha_{i j} \mathbf T_{cw} \left[\begin{array}{c} \mathbf{c}_{j}^{w} \\ 1 \end{array}\right] \\ &=\sum_{j=1}^{4} \alpha_{i j}\left[\begin{array}{c} \mathbf{c}_{j}^{c} \\ 1 \end{array}\right] \end{aligned} [pic1]=Tcw[piw1]=Tcw ∑j=14αijcjw∑j=14αij =j=1∑4αijTcw[cjw1]=j=1∑4αij[cjc1](09)所以结论(08)成立。但是这里需要注意，以上推导建立在 ∑ j = 1 4 α i j = 1 \sum_{j=1}^{4} \alpha_{i j}=1 ∑j=14αij=1 这个重要的约束条件上，如果没有这个约束，那么上述结论不成立。

到目前为止，我们已经根据世界坐标系下3D点 P i w \mathbf P_i^w Piw 求出了世界坐标系下的4个控制点 c j w , j = 1 , ⋯ , 4 \mathbf{c}_{j}^{w}, j=1, \cdots, 4 cjw,j=1,⋯,4, 以及每个3D点对应的控制点系数 α i j \alpha_{i j} αij, 前面证明过: 同一个3D点在世界坐标系下对应控制点的系数 α i j \alpha_{i j} αij 和其在相机坐标系下对应控制点的系数相同。所以如果我们能把4个控制点在相机坐标系下的坐标 c j c , j = 1 , ⋯ , 4 \mathbf{c}_{j}^{c}, j=1, \cdots, 4 cjc,j=1,⋯,4 求解出来，就可以得到世界坐标系下3D点在相机坐标系下的坐标 P j c \mathbf{P}_{j}^{c} Pjc。这样就可以根据ICP求解位姿了。

六、透视投影关系构建约束

记 w i w_i wi为投影尺度系数， K \mathbf K K相机内参矩阵， u \mathbf u u为相机坐标系下3D参考点对应的2D投影坐标，根据相机投影原理可得： w i [ u i 1 ] = K p i c = K ∑ j = 1 4 α i j c j c (10) \color{Green} \tag{10} w_{i}\left[\begin{array}{c} \mathbf{u}_{i} \\ \\ 1\end{array}\right]=\mathbf{K} \mathbf{p}_{i}^{c}=\mathbf{K} \sum_{j=1}^{4} \alpha_{i j} \mathbf{c}_{j}^{c} wi ui1 =Kpic=Kj=1∑4αijcjc(10)记控制点 c j c c_j^c cjc 坐标为 [ x j c , y j c , z j c ] T [x_j^c,y_j^c,z_j^c]^T [xjc,yjc,zjc]T, f u , f v f_u,f_v fu,fv是焦距， u c , v c u_c,v_c uc,vc是主点坐标，上式可以化为：
w i [ u i v i 1 ] = [ f u 0 u c 0 f v v c 0 0 1 ] ∑ j = 1 4 α i j [ x j c y j c z j c ] (11) \color{Green} \tag{11} w_{i}\left[\begin{array}{c} u_{i} \\ v_{i} \\ 1 \end{array}\right]=\left[\begin{array}{ccc} f_{u} & 0 & u_{c} \\ 0 & f_{v} & v_{c} \\ 0 & 0 & 1 \end{array}\right] \sum_{j=1}^{4} \alpha_{i j}\left[\begin{array}{c} x_{j}^{c} \\ y_{j}^{c} \\ z_{j}^{c} \end{array}\right] wi uivi1 = fu000fv0ucvc1 j=1∑4αij xjcyjczjc (11)根据最后一行可以推出 w i = ∑ j = 1 4 ( α i j z j c ) , i = 1 , ⋯ , n (12) \color{Green} \tag{12} w_{i}=\sum_{j=1}^{4} (\alpha_{i j} z_{j}^{c}), \quad i=1, \cdots, n wi=j=1∑4(αijzjc),i=1,⋯,n(12)消去最后一行，我们把上面矩阵展开写成等式右边为0的表达式，所以实际上每个点对可以得到2个方程: ∑ j = 1 4 ( α i j f u x j c + α i j ( u c − u i ) z j c ) = 0 ∑ j = 1 4 ( α i j f v y j c + α i j ( v c − v i ) z j c ) = 0 (12) \color{Green} \tag{12} \begin{array}{l} \sum_{j=1}^{4}(\alpha_{i j} f_{u} x_{j}^{c}+\alpha_{i j}\left(u_{c}-u_{i}\right) z_{j}^{c})=0 \\ \\ \sum_{j=1}^{4}(\alpha_{i j} f_{v} y_{j}^{c}+\alpha_{i j}\left(v_{c}-v_{i}\right) z_{j}^{c}) =0 \end{array} ∑j=14(αijfuxjc+αij(uc−ui)zjc)=0∑j=14(αijfvyjc+αij(vc−vi)zjc)=0(12)这里的待求的未知数是12个相机坐标系下控制点坐标 { ( x j c , y j c , z j c ) } , j = 1 , ⋯ , 4 \left\{\left(x_{j}^{c}, y_{j}^{c}, z_{j}^{c}\right)\right\}, j=1, \cdots, 4 {(xjc,yjc,zjc)},j=1,⋯,4，我们把个匹配点对全部展开，再写成矩阵的形式： [ α 11 f u 0 α 11 ( u c − u 1 ) ⋯ α 14 f u 0 α 14 ( u c − u 1 ) 0 α 11 f v α 11 ( v c − v 1 ) ⋯ 0 α 14 f v α 14 ( v c − v 1 ) ⋮ α i 1 f u 0 α i 1 ( u c − u i ) ⋯ α i 4 f u 0 α i 4 ( u c − u i ) 0 α i 1 f v α i 1 ( v c − v i ) ⋯ 0 α i 4 f v α i 4 ( v c − v i ) ⋮ α n 1 f u 0 α n 1 ( u c − u n ) ⋯ α n 4 f u 0 α n 4 ( u c − u n ) 0 α n 1 f v α n 1 ( v c − v n ) ⋯ 0 α n 4 f v α n 4 ( v c − v n ) ] [ x 1 c y 1 c z 1 c x 2 c y 2 c z 2 c x 3 c y 3 c z 3 c x 4 c y 4 c z 4 c ] = 0 (13) \color{Green} \tag{13} \left[\begin{array}{ccccccc} \alpha_{11} f_{u} & 0 & \alpha_{11}\left(u_{c}-u_{1}\right) & \cdots & \alpha_{14} f_{u} & 0 & \alpha_{14}\left(u_{c}-u_{1}\right) \\ 0 & \alpha_{11} f_{v} & \alpha_{11}\left(v_{c}-v_{1}\right) & \cdots & 0 & \alpha_{14} f_{v} & \alpha_{14}\left(v_{c}-v_{1}\right) \\ \vdots & & & & & & \\ \alpha_{i 1} f_{u} & 0 & \alpha_{i 1}\left(u_{c}-u_{i}\right) & \cdots & \alpha_{i 4} f_{u} & 0 & \alpha_{i 4}\left(u_{c}-u_{i}\right) \\ 0 & \alpha_{i 1} f_{v} & \alpha_{i 1}\left(v_{c}-v_{i}\right) & \cdots & 0 & \alpha_{i 4} f_{v} & \alpha_{i 4}\left(v_{c}-v_{i}\right) \\ \vdots & & & & & & \\ \alpha_{n 1} f_{u} & 0 & \alpha_{n 1}\left(u_{c}-u_{n}\right) & \cdots & \alpha_{n 4} f_{u} & 0 & \alpha_{n 4}\left(u_{c}-u_{n}\right) \\ 0 & \alpha_{n 1} f_{v} & \alpha_{n 1}\left(v_{c}-v_{n}\right) & \cdots & 0 & \alpha_{n 4} f_{v} & \alpha_{n 4}\left(v_{c}-v_{n}\right) \end{array}\right]\left[\begin{array}{l} x_{1}^{c} \\ y_{1}^{c} \\ z_{1}^{c} \\ x_{2}^{c} \\ y_{2}^{c} \\ z_{2}^{c} \\ x_{3}^{c} \\ y_{3}^{c} \\ z_{3}^{c} \\ x_{4}^{c} \\ y_{4}^{c} \\ z_{4}^{c} \end{array}\right]=0 α11fu0⋮αi1fu0⋮αn1fu00α11fv0αi1fv0αn1fvα11(uc−u1)α11(vc−v1)αi1(uc−ui)αi1(vc−vi)αn1(uc−un)αn1(vc−vn)⋯⋯⋯⋯⋯⋯α14fu0αi4fu0αn4fu00α14fv0αi4fv0αn4fvα14(uc−u1)α14(vc−v1)αi4(uc−ui)αi4(vc−vi)αn4(uc−un)αn4(vc−vn) x1cy1cz1cx2cy2cz2cx3cy3cz3cx4cy4cz4c =0(13)其中， i = 1 , ⋯ , n i=1,\cdots,n i=1,⋯,n 表示点对的数目，记左边第一个矩阵为 M \mathbf M M，它的大小为 2n x12，第二个矩阵x是带求未知量组成的矩阵，大小为 12 x 1，则上式可以写成：
M x = 0 (14) \color{Green} \tag{14} \mathbf M \mathbf x=0 Mx=0(14)

七、结语

通过前面的推导，最终获得矩阵方程 M x = 0 \mathbf M \mathbf x=0 Mx=0，其中 x \mathbf x x 就是由 c j c = { ( x j c , y j c , z j c ) } , j = 1 , ⋯ , 4 c_j^c=\left\{\left(x_{j}^{c}, y_{j}^{c}, z_{j}^{c}\right)\right\}, j=1, \cdots, 4 cjc={(xjc,yjc,zjc)},j=1,⋯,4 组合而成的列向量，也就是说其为我们需要求解的世界坐标系下3D点在相机坐标系下的坐标。