本文原创，转载请说明地址：https://blog.csdn.net/shanpenghui/article/details/110003959

一、前言

ORBSLAM3单目视觉有很多知识点需要展开和深入，初始化过程是必然要经历的，而网上资料不够系统，因此本文主旨是从代码实现出发，把初始化过程系统化，建立起知识树，以把零碎的知识点串联起来，方便快速学习提升自己。注意，本文虽然从代码出发，但并非讲全部代码细节，如有需要建议直接看源代码，地址是：https://github.com/UZ-SLAMLab/ORB_SLAM3，我自己稍微做了点修改，可以跑数据集的版本，可以参考一下，地址是：https://github.com/shanpenghui/ORB_SLAM3_Fixed

二、初始化主要函数

ORBSLAM单目视觉SLAM的追踪器接口是函数TrackMonocular，调用了GrabImageMonocular，其下面有2个主要的函数：Frame::Frame()和Tracking::Track()，本文和上篇都是按照以下框架流程来分解单目初始化过程，上篇记录了Frame::Frame()，本文就记录Tracking::Track()。

1. Tracking作用

ORB-SLAM3的Tracking部分作用论文已提及，包含输入当前帧、初始化、相机位姿跟踪、局部地图跟踪、关键帧处理、姿态更新与保存等，如图。

2. 两个主要函数

单目地图初始化函数是Tracking::MonocularInitialization，其主要是调用以下两个函数完成了初始化过程，ORBmatcher::SearchForInitialization和KannalaBrandt8::ReconstructWithTwoViews，前者用于参考帧和当前帧的特征点匹配，后者利用构建的虚拟相机模型，针对不同相机计算基础矩阵和单应性矩阵，选取最佳的模型来恢复出最开始两帧之间的相对姿态，并进行三角化得到初始地图点。

三、ORBmatcher::SearchForInitialization

这个函数的主要作用是构建旋转角度直方图，选取最优的三个Bin，也就是占据概率最大的三个Bin，如图（数字3被异形吃掉了^-^）。因为当前帧会提取到诸多特征点，每一个都可以作为图像旋转角度的测量值，我们希望能在诸多的角度值中，选出最能代表当前帧的旋转角度的测量值，这就是为什么要在旋转角度直方图中选最优的3个Bin的原因。

旋转角度在代码中哪里计算呢？就是在计算描述子的时候算的，调用函数IC_Angle，代码见ORBextractor.cc#L475，也在下面列出。感兴趣的同学想知道为什么要这么麻烦的选取最优3个角度，请从旋转不变性开始理解，原理参见《3-5-3如何保证描述子旋转不变性?》

keypoint->angle = IC_Angle(image, keypoint->pt, umax);

四、KannalaBrandt8::ReconstructWithTwoView

1. 畸变校正

利用鱼眼模型，对两帧图像的特征点进行畸变校正，代码见KannalaBrandt8.cpp#L219。要注意的是，鱼眼模型的特殊性在于只考虑径向畸变，忽略切向畸变，所以其 p i p_i pi值都是0。想要深入理解鱼眼模型的同学可以参考这篇文章《鱼眼相机成像模型
》。ORB-SLAM3中对不同模型相机的畸变校正做了区分，当相机模型是针孔的时候，用的畸变校正参数是mDistCoef，当相机模型是鱼眼的时候，用的是虚拟出的相机类，代码参见mpCamera = new KannalaBrandt8(vCamCalib)，为避免重复校正，用了个条件限制，就是在函数Frame::UndistortKeyPoints中判断mDistCoef.at<float>(0)==0.0，代码参见Frame.cc#L734，因为在用鱼眼相机模型的时候，mDistCoef没有赋值，都是0。

cv::fisheye::undistortPoints(vPts1,vPts1,K,D,R,K);
cv::fisheye::undistortPoints(vPts2,vPts2,K,D,R,K);

2. 位姿估计

主要由函数TwoViewReconstruction::Reconstruct完成，涉及到的知识点又多又关键的，包括对极约束、八点法、归一化、直接线性变换、卡方检验、重投影等，先从主要流程开始理解。

2.1 主流程

利用随机种子
DUtils::Random::SeedRandOnceTwoViewReconstruction.cc#L79，在所有匹配特征点对中随机选择8对匹配特征点为一组for(size_t j=0; j<8; j++)TwoViewReconstruction.cc#L86，用于估计H矩阵和F矩阵。
将当前帧和参考帧中的特征点坐标进行归一化。TwoViewReconstruction::NormalizeTwoViewReconstruction.cc#L753
用DLT方法求解F矩阵
TwoViewReconstruction::ComputeF21TwoViewReconstruction.cc#L273
对给定的F矩阵打分,需要使用到卡方检验的知识
TwoViewReconstruction::CheckFundamentalTwoViewReconstruction.cc#L395
利用得到的最佳模型（选择得分较高的矩阵值，单应矩阵H或者基础矩阵F）估计两帧之间的位姿，代码中对应着函数ReconstructH或ReconstructF。其中，分两个步骤。第一是利用基础矩阵F和本质矩阵E的关系 F = K − T E K − 1 F=K^{-T}EK^{-1} F=K−TEK−1，计算出四组解。第二是调用的函数CheckRT作用是用R，t来对特征匹配点三角化，并根据三角化结果判断R,t的合法性。最终可以得到最优解的条件是位于相机前方的3D点个数最多并且三角化视差角必须大于最小视差角，这个也和作者在论文中说的

2.2 对极约束(原理基础)

2.2.1 加速匹配特征点

通过两帧图像估计相机位姿的背后原理是什么？因为是单目初始化，我们现有的条件是已知 2D 的像素坐标，目的是根据两幅图像间多组2D像素点对来估计相机的运动。但是，有那么多像素点，我们如果都要遍历的话太慢了，有没有什么办法能缩小搜索范围呢？有！就是利用对极约束，如果要搜索左图上点 x x x在右图上的匹配点 x ′ x' x′，则在对应的极线 l ′ l' l′上搜索即可，这就大大减少了在右图中寻找匹配点的搜索范围，减少为区区一条线，而这也是对极约束的应用意义之一。

2.2.2 对极约束是将空间点 p p p投影在图像上的(特征)点约束在极线(和相机中心点相关)上的一种方法

关于对极约束，我们来看示意图：

其中，极点、极平面、极线概念非常重要：

极点(Epipole)：分别是左边相机中心 o o o在右图像平面上的像 e ′ e' e′，右相机中心 o ′ o' o′在左像平面上的像e。
极平面(Epipolar plane)：两个相机中心 o o o、 o ′ o' o′和空间中某点 p p p形成的平面。
极线(Epipolar line)：极平面分别和两个像平面的交线 l l l、 l ′ l' l′。

2.2.3 对极约束描述了点与线的关系

试想一下，从左图的点 o o o往点 p p p看，如果不知道点 p p p的深度信息，可以把点 p p p看成在射线 o p op op上的任意点，因为该射线上的点都会投影到同一个像素点 x x x上。
同理，当我们不知道点 p p p的深度信息，从右图的点 o ′ o' o′往点 p p p看，可以把点 p p p看成在射线 o ′ p o'p o′p上的任意点，因为该射线上的点都会投影到同一个像素点 x ′ x' x′上。如下图所示：

2.2.4 基础矩阵Fundamental 代数推导

有了以上的示意，我们尝试用数学公式描述极点、极线和极平面之间的关系。看了好几篇文章，感觉还是视觉十四讲里面的代数推导比较明晰，我就直接参考过来，当做记录了，其他比较杂乱，记录在《SLAM 学习笔记本质矩阵E、基础矩阵F、单应矩阵H的推导》，感兴趣的同学可以看看。
设以第一个相机作为坐标系三维空间的点：
P = [ X Y Z ] T P= \begin{bmatrix} X & Y& Z \end{bmatrix} ^T P=[XYZ]T

根据针孔相机模型,我们知道两个像素点 p 1 , p 2 p_1,p_2 p1,p2的像素位置为
s 1 p 1 = K P s_1p_1=KP s1p1=KP s 2 p 2 = K ( R P + t ) s_2p_2=K(RP+t) s2p2=K(RP+t)

这里 K K K为相机内参矩阵 R , t R,t R,t为两个坐标系的相机运动(如果我们愿意,也可以写成李代数形式)。如果使用齐次坐标，我们也可以把上式写成在乘以非零常数下成立的(up

公式(7.9)非常重要，在后面计算的时候需要用到。

2.2.5 对极约束简化了相机位姿估计

对极约束简洁地给出了两个匹配点的空间位置关系。于是,相机位姿估计问题变为以下两步：

根据配对点的像素位置,求出 E E E或者 F F F。
根据 E E E或者 F F F，求出 R , t R,t R,t。

由于 E E E和 F F F只相差了相机内参，而内参在 SLAM 中通常是已知的。

2.2.6 结尾

为了不混乱，先写基础矩阵F的理论和代码，单应矩阵H部分放后面讨论。加上篇幅限制，就不再展开了，这里可以参考另外几篇比较好的文章，有比较详细的推导过程，想深入研究的童鞋可以看看。
1、SLAM入门之视觉里程计(3)：两视图对极约束基础矩阵
2、SLAM基础知识总结

2.3 八点法(求解基础)

上面讨论了一大堆基础理论，介绍了对极约束，推导了基础矩阵F，我们也知道基础矩阵F是对极约束关系的代数表示，并且对极约束关系独立与场景的结构，只依赖与相机的内参和外参（相对位姿）。这样可以通过匹配的像点对计算出两幅图像的基础矩阵，然后分解基础矩阵F得到相机的相对位姿。

2.3.1 通过匹配点对估算基础矩阵F

基础矩阵表示的是图像中的像点p1到另一幅图像对极线l2的映射，有如下公式：
l 2 = F p 1 (1) l_2=Fp_1\tag{1} l2=Fp1(1)
而和像点 p 1 p_1 p1匹配的另一个像点 p 2 p_2 p2必定在对极线 l 2 l_2 l2上，所以就有：
p 2 T l 2 = p 2 T F p 1 = 0 (2) p_2^Tl_2=p_2^TFp_1=0\tag{2} p2Tl2=p2TFp1=0(2)
这样仅通过匹配的点对，就可以计算出两视图的基础矩阵 F F F，基础矩阵 F F F是一个 3×3的矩阵，有9个未知元素。然而，上面的公式中使用的齐次坐标，齐次坐标在相差一个常数因子下是相等， F F F也就只有8个未知元素，也就是说，只需要8对匹配的点对就可以求解出两视图的基础矩阵 F F F。

2.3.2 八点法方程式

看看怎么用八点法计算基础矩阵F，本文就不展开单应矩阵H的计算过程了。
假设一对匹配的像点 p 1 = [ u 1 , v 1 , 1 ] T p_1=\begin{bmatrix}u_1,v_1,1\end{bmatrix}^T p1=[u1,v1,1]T， p 2 = [ u 2 , v 2 , 1 ] T p_2=\begin{bmatrix}u_2,v_2,1\end{bmatrix}^T p2=[u2,v2,1]T，想要计算点 p 1 → p 2 p_1\rightarrow p_2 p1→p2，就是参考帧上的点 p 1 p_1 p1到当前帧上的点 p 2 p_2 p2的关系TwoViewReconstruction::ComputeF21TwoViewReconstruction.cc#L281，带入式子中，得到：
[ u 2 , v 2 , 1 ] [ f 1 f 2 f 3 f 4 f 5 f 6 f 7 f 8 f 9 ] [ u 1 v 1 1 ] = 0 (3) \begin{bmatrix} u_2,v_2,1 \end{bmatrix} \begin{bmatrix} f_1 & f_2 & f_3 \\ f_4 & f_5 & f_6 \\ f_7 & f_8 & f_9 \end{bmatrix} \begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix}=0 \tag{3} [u2,v2,1]⎣⎡f1f4f7f2f5f8f3f6f9⎦⎤⎣⎡u1v11⎦⎤=0(3)
把基础矩阵 F F F的各个元素当作一个向量处理
f = [ f 1 , f 2 , f 3 , f 4 , f 5 , f 6 , f 7 , f 8 , f 9 ] (4) f= \begin{bmatrix} f_1,f_2,f_3,f_4,f_5,f_6,f_7,f_8,f_9 \end{bmatrix} \tag{4} f=[f1,f2,f3,f4,f5,f6,f7,f8,f9](4)
那么上面式子可以写为
[ u 2 u 1 , u 2 v 1 , u 2 , v 2 u 1 , v 2 v 1 , v 2 , u 1 , v 1 , 1 ] ⋅ f = 0 (5) \begin{bmatrix} u_2u_1,u_2v_1,u_2,v_2u_1,v_2v_1,v_2,u_1,v_1,1 \end{bmatrix} \cdot f=0 \tag{5} [u2u1,u2v1,u2,v2u1,v2v1,v2,u1,v1,1]⋅f=0(5)
对于其他的点对也使用同样的表示方法。这样将得到的所有方程放到一起，得到一个线性方程组( ( u i v i ) (u^{i} v^{i}) (uivi)表示第 i i i个特征点)
[ u 2 1 u 1 1 , u 2 1 v 1 1 , u 2 1 , v 2 1 u 1 1 , v 2 1 v 1 1 , v 2 1 , u 1 1 , v 1 1 , 1 u 2 2 u 1 2 , u 2 2 v 1 2 , u 2 2 , v 2 2 u 1 2 , v 2 2 v 1 2 , v 2 2 , u 1 2 , v 1 2 , 1 u 2 3 u 1 3 , u 2 3 v 1 3 , u 2 3 , v 2 3 u 1 3 , v 2 3 v 1 3 , v 2 3 , u 1 3 , v 1 3 , 1 ⋯ u 2 8 u 1 8 , u 2 8 v 1 8 , u 2 8 , v 2 8 u 1 8 , v 2 8 v 1 8 , v 2 8 , u 1 8 , v 1 8 , 1 ] ⋅ [ f 1 f 2 f 3 f 4 f 5 f 6 f 7 f 8 f 9 ] = 0 (6) \begin{bmatrix} u_2^1u_1^1,u_2^1v_1^1,u_2^1,v_2^1u_1^1,v_2^1v_1^1,v_2^1,u_1^1,v_1^1,1 \\ u_2^2u_1^2,u_2^2v_1^2,u_2^2,v_2^2u_1^2,v_2^2v_1^2,v_2^2,u_1^2,v_1^2,1 \\ u_2^3u_1^3,u_2^3v_1^3,u_2^3,v_2^3u_1^3,v_2^3v_1^3,v_2^3,u_1^3,v_1^3,1 \\ \cdots \\ u_2^8u_1^8,u_2^8v_1^8,u_2^8,v_2^8u_1^8,v_2^8v_1^8,v_2^8,u_1^8,v_1^8,1 \\ \end{bmatrix} \cdot \begin{bmatrix} f_1 \\ f_2 \\ f_3 \\ f_4 \\ f_5 \\ f_6 \\ f_7 \\ f_8 \\ f_9 \end{bmatrix}=0 \tag{6} ⎣⎢⎢⎢⎢⎡u21u11,u21v11,u21,v21u11,v21v11,v21,u11,v11,1u22u12,u22v12,u22,v22u12,v22v12,v22,u12,v12,1u23u13,u23v13,u23,v23u13,v23v13,v23,u13,v13,1⋯u28u18,u28v18,u28,v28u18,v28v18,v28,u18,v18,1⎦⎥⎥⎥⎥⎤⋅⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡f1f2f3f4f5f6f7f8f9⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=0(6)

2.3.3 直接用八点法求解太难了

求解上面的方程组就可以得到基础矩阵各个元素了。当然这只是理想中的情况，由于噪声、数值的舍入误差和错误的匹配点的影响，仅仅求解上面的线性方程组得到的基础矩阵非常的不稳定，因此在8点法的基础上有各种改进方法。

这就要涉及到接下来的2个小节：

归一化《2-2-3 Normalize》
直接线性变换《2-2-4 DLT(Direct Linear Transform)》

2.4 Normalize(减少噪声)

2.4.1 什么是归一化？

归一化方法有两种形式，一种是把数变为（0，1）之间的小数，一种是把有量纲表达式变为无量纲表达式。主要是为了数据处理方便提出来的，把数据映射到0～1范围之内处理，更加便捷快速，应该归到数字信号处理范畴之内。

在这里，归一化特征点到同一尺度，作为后续DLT直接线性变换的输入。
[ x ′ y ′ 1 ] ⊺ = T ∗ [ x y 1 ] ⊺ (7) \left[ \begin{matrix} x' \\ y' \\ 1 \end{matrix} \right]^\intercal = T * \left[ \begin{matrix} x \\ y \\ 1 \end{matrix} \right]^\intercal \tag{7} ⎣⎡x′y′1⎦⎤⊺=T∗⎣⎡xy1⎦⎤⊺(7)

2.4.2 为什么要归一化？

在相似变换之后(点在不同的坐标系下)，他们的单应性矩阵是不相同的。如果图像存在噪声,使得点的坐标发生了变化,那么它的单应性矩阵也会发生变化。我们采取的方法是将点的坐标放到同一坐标系下,并将缩放尺度也进行统一。对同一幅图像的坐标进行相同的变换，不同图像进行不同变换。缩放尺度是为了让噪声对于图像的影响在一个数量级上。这样子做可以在同一坐标系下对比点，避免因为噪声的不同导致的单应矩阵变化，从而减少噪声对图像的影响。

2.4.3 怎么归一化？

这要涉及到一阶矩和二阶矩的计算。
一阶矩就是随机变量的期望，二阶矩就是随机变量平方的期望。一阶绝对矩定义为变量与均值绝对值的平均。
设存在多个变量：
u 1 , u 2 , . . . , u N u_1,u_2,...,u_N u1,u2,...,uN
其期望：
u ‾ = ∑ i = 0 N ∣ x i − u ‾ ∣ N (8) \overline u=\frac{\sum_{i=0}^N|x_i-\overline u|}{N}\tag{8} u=N∑i=0N∣xi−u∣(8)
新的N维变量是：
u ′ = u − u ‾ ∣ u ‾ ∣ (9) u'=\frac{u-\overline u}{|\overline u|}\tag{9} u′=∣u∣u−u(9)
ORB-SLAM3程序中用了一个中间变量sX = 1.0/meanDevX;和sY = 1.0/meanDevY;，具体的公式是：
[ x ′ y ′ 1 ] = [ s X 0 − X m e a n s X 0 s Y − Y m e a n s Y 0 0 1 ] [ x y 1 ] (10) \begin{bmatrix} x' \\ y' \\ 1 \end{bmatrix} = \begin{bmatrix} sX & 0 & -X_{mean}sX \\ 0 & sY & -Y_{mean}sY \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ 1 \end{bmatrix} \tag{10} ⎣⎡x′y′1⎦⎤=⎣⎡sX000sY0−XmeansX−YmeansY1⎦⎤⎣⎡xy1⎦⎤(10)
归一化对应的函数是TwoViewReconstruction::NormalizeTwoViewReconstruction.cc#L753，其中公式和代码对应的变量分别是：

公式	程序
x ′ x' x′	v N o r m a l i z e d P o i n t s [ i ] . x vNormalizedPoints[i].x vNormalizedPoints[i].x
y ′ y' y′	v N o r m a l i z e d P o i n t s [ i ] . y vNormalizedPoints[i].y vNormalizedPoints[i].y
x x x	v N o r m a l i z e d P o i n t s [ i ] . x vNormalizedPoints[i].x vNormalizedPoints[i].x
y y y	v N o r m a l i z e d P o i n t s [ i ] . y vNormalizedPoints[i].y vNormalizedPoints[i].y
s X sX sX	s X sX sX
s Y sY sY	s Y sY sY
X m e a n X_{mean} Xmean	m e a n D e v X meanDevX meanDevX
Y m e a n Y_{mean} Ymean	m e a n D e v Y meanDevY meanDevY

2.5 Direct Linear Transform(求最优解)

上面讲了用归一化的方法来降低噪声，但仅仅对图像坐标进行归一化处理，能在一定程度上提高计算的精度，但在实践中还是不够的。八点法中，只使用8对匹配的点对估计基础矩阵，但通常两幅图像的匹配的点对远远多于8对，可以利用更多匹配的点对来求解上面的方程。
由于基础矩阵F在一个常量因子下是等价的(意思就是乘以和除以一个常量应该对它没有影响)，这样可以给基础矩阵F的元素组成的向量 f f f施加一个约束条件，即 f f f的1-范数为1：
∣ ∣ f ∣ ∣ 1 = 1 (11) ||f||_1=1\tag{11} ∣∣f∣∣1=1(11)

向量范数
对于向量

1-范数的定义是：

即向量元素绝对值之和，x 到零点的曼哈顿距离

矩阵范数
对于矩阵

1-范数的定义是：

即所有矩阵列向量绝对值之和的最大值

我觉得公式(22)可以理解为多项式中，每个变量的系数的绝对值之和为1

这样由 m ≥ 8 m\geq 8 m≥8个匹配的点对，组合成一个矩阵 A m × 9 A_{m×9} Am×9，求解上面方程(6)

[ u 2 1 u 1 1 , u 2 1 v 1 1 , u 2 1 , v 2 1 u 1 1 , v 2 1 v 1 1 , v 2 1 , u 1 1 , v 1 1 , 1 u 2 2 u 1 2 , u 2 2 v 1 2 , u 2 2 , v 2 2 u 1 2 , v 2 2 v 1 2 , v 2 2 , u 1 2 , v 1 2 , 1 u 2 3 u 1 3 , u 2 3 v 1 3 , u 2 3 , v 2 3 u 1 3 , v 2 3 v 1 3 , v 2 3 , u 1 3 , v 1 3 , 1 ⋯ u 2 8 u 1 8 , u 2 8 v 1 8 , u 2 8 , v 2 8 u 1 8 , v 2 8 v 1 8 , v 2 8 , u 1 8 , v 1 8 , 1 ] ⋅ [ f 1 f 2 f 3 f 4 f 5 f 6 f 7 f 8 f 9 ] = 0 (6) \begin{bmatrix} u_2^1u_1^1,u_2^1v_1^1,u_2^1,v_2^1u_1^1,v_2^1v_1^1,v_2^1,u_1^1,v_1^1,1 \\ u_2^2u_1^2,u_2^2v_1^2,u_2^2,v_2^2u_1^2,v_2^2v_1^2,v_2^2,u_1^2,v_1^2,1 \\ u_2^3u_1^3,u_2^3v_1^3,u_2^3,v_2^3u_1^3,v_2^3v_1^3,v_2^3,u_1^3,v_1^3,1 \\ \cdots \\ u_2^8u_1^8,u_2^8v_1^8,u_2^8,v_2^8u_1^8,v_2^8v_1^8,v_2^8,u_1^8,v_1^8,1 \\ \end{bmatrix} \cdot \begin{bmatrix} f_1 \\ f_2 \\ f_3 \\ f_4 \\ f_5 \\ f_6 \\ f_7 \\ f_8 \\ f_9 \end{bmatrix}=0 \tag{6} ⎣⎢⎢⎢⎢⎡u21u11,u21v11,u21,v21u11,v21v11,v21,u11,v11,1u22u12,u22v12,u22,v22u12,v22v12,v22,u12,v12,1u23u13,u23v13,u23,v23u13,v23v13,v23,u13,v13,1⋯u28u18,u28v18,u28,v28u18,v28v18,v28,u18,v18,1⎦⎥⎥⎥⎥⎤⋅⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡f1f2f3f4f5f6f7f8f9⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=0(6)

就变成了求解如下问题的最小二乘解：
min ⁡ ∣ ∣ f ∣ ∣ = 1 ∣ ∣ A f ∣ ∣ 2 (12) \min \limits_{||f||=1} ||Af||^2\tag{12} ∣∣f∣∣=1min∣∣Af∣∣2(12)
其中，矩阵 A A A的每一行来自一对匹配点；f是基础矩阵 F元素构成的待求解的向量，根据2-范数的定义：
∥ A f ∥ 2 = ( A f ) T ( A f ) = f T ( A T A ) f (13) ∥Af∥^2=(Af)^T(Af)=f^T(A^TA)f\tag{13} ∥Af∥2=(Af)T(Af)=fT(ATA)f(13)
将上式的中间部分提取出来得到矩阵 M = A T A M=A^TA M=ATA,这是一个 9×9的矩阵。基于拉格朗日-欧拉乘数优化定理，在 ∣ ∣ f ∣ ∣ = 1 ||f||=1 ∣∣f∣∣=1约束下， A f = 0 Af=0 Af=0的最小二乘解，为矩阵 M = A T A M=A^TA M=ATA的最小特征值对应的特征向量。

这里就用到了大学学的线性代数的理论，针对后悔没有好好地学习，现在充下电。这个问题其实可以抽象成下面这个数学问题：

证明：当 ||X||＝1时，AX=0的最小二乘解是 A T A A^TA ATA的最小特征值对应的特征向量。

证明过程其实大家在大学应该都推理过，这里不赘述了，感兴趣的请自行搜索证明过程。在公式(23)里面，未知量是 f f f，已知量是 Q Q Q，利用数学问题的结论，带入实际问题就是

当 ∣ ∣ f ∣ ∣ = 1 ||f||=1 ∣∣f∣∣=1的时候， A f = 0 Af=0 Af=0的最小二乘解是 A T A A^TA ATA的最小特征值对应的特征向量。

而 A f = 0 Af=0 Af=0也是齐次线性方程，这个问题终究回归到当初我们上大学的时候逃过的那些线性代数和高数的课。咳咳，跑题了，言归正传。

那么齐次线性方程的最小二乘解和奇异值分解有什么关系呢？

这里A相当于Q，而Q是K×9的矩阵，其中m=K，n=9，而K>9，所以m>n，所以这个问题也是超定问题。这种方程一般来说无解，但可求其最小二乘解，即所谓的最小二乘问题（Least Square Problem）。

所以可以对矩阵 A A A进行奇异值分解(SVD)
A = U Σ V T (14) A=U\Sigma V^T\tag{14} A=UΣVT(14)
其中， A是 m × 9 m×9 m×9的矩阵、U是K×K的正交阵、 Σ \Sigma Σ是 m × 9 m×9 m×9的对角矩阵，对角线的元素是奇异值、 V T V^T VT是 9×9的正交阵，每一个列向量对应着 Σ \Sigma Σ中的奇异值。所以，最小二乘解就是 V T V^T VT的第9个列向量（为什么呢？？）

我们要先清楚，怎么用奇异值求线性最小二乘解呢？
用奇异值分解在求线性最小二乘解的时候，我们找一个向量x使得 ∥ A x − b ∥ ∥Ax−b∥ ∥Ax−b∥最小，我们可以化为（矩阵U具有保范性）
∥ A x − b ∥ = ∥ U D V T x − b ∥ = ∥ D V T x − U T b ∥ ∥Ax−b∥=∥UDV^Tx−b∥=∥DV^Tx−U^Tb∥ ∥Ax−b∥=∥UDVTx−b∥=∥DVTx−UTb∥
令 y = V T x y=V^Tx y=VTx， b ′ = U T b b'=U^Tb b′=UTb。则问题变成最小化 ∥ D y − b ′ ∥ ∥Dy−b'∥ ∥Dy−b′∥， D = [ Σ 0 ] D=\begin{bmatrix}\Sigma \\ 0\end{bmatrix} D=[Σ0]是除对角线线元素以外全是0的 m × n m×n m×n矩阵。可以把该方程写成如下形式：

而现在，用超过8个匹配点来求解矩阵Q的情况，是求解齐次线性方程的最小二乘解，形如 A x = 0 Ax=0 Ax=0。当然，最简单的解就是 x = 0 x=0 x=0，但这并不是我们想要的解。我们需要的是非零解。对于上面的超定方程，我们可以找到近似满足方程的非零解。对于解x，我们令 y = V T x y=V^Tx y=VTx，把问题转换为最小化 ∥ D y ∥ ∥Dy∥ ∥Dy∥，做进一步处理，可以转换为最小化 ∥ y T D T D y ∥ ∥y^TD^TDy∥ ∥yTDTDy∥。有：

我们知道， y y y若是解，乘以任意倍数的 k y ky ky也是方程的解，所以，我们不妨增加约束，取 ∥ y ∥ = 1 ∥y∥=1 ∥y∥=1，对应着我们上面的条件 ∥ f ∥ = 1 ∥f∥=1 ∥f∥=1。
观察上述等式，由于 σ 1 ≥ σ 2 ≥ ⋯ ≥ σ n ≥ 0 σ1≥σ2≥⋯≥σn≥0 σ1≥σ2≥⋯≥σn≥0（认真看！这里是想说的就是 σ n σn σn最小，那么我们想要得到最小值，只要在满足 ∥ y ∥ = 1 ∥y∥=1 ∥y∥=1的条件下，取到 σ n σn σn这个最小值不就可以了吗？）。所以，我们很容易得到，当我们取 y = [ 0 , 0 , … , 1 ] T y=[0,0,…,1]^T y=[0,0,…,1]T时，得的 ∥ y T D T D y ∥ ∥y^TD^TDy∥ ∥yTDTDy∥最小。所以原始的齐次方程的解为：

也就是说超定方程 A x = 0 Ax=0 Ax=0的最小二乘解为A奇异值分解后，V的最后一列向量，这回明白了吗？

也就是可由向量 f = V 9 f=V_9 f=V9构造基础矩阵 F。

我们不难发现，不仅有约束条件 ∥ f ∥ = 1 ∥f∥=1 ∥f∥=1可以利用，基础矩阵F还有一个重要的性质，可以作为进一步的约束条件。那就是基础矩阵 F的秩为2，
R a n k ( F ) = 2 (15) Rank(F)=2\tag{15} Rank(F)=2(15)

上述使用列向量 V 9 V_9 V9构造的基础矩阵的秩通常不为2，需要进一步的优化。在估计基础矩阵时，设其最小奇异值为0，对上面方法取得的基础矩阵再进行一次SVD分解
F = S V D = S [ v 1 0 0 0 v 2 0 0 0 v 3 ] D (16) F=SVD=S \begin{bmatrix} v_1 & 0 & 0 \\ 0 & v_2 & 0 \\ 0 & 0 & v_3 \end{bmatrix}D \tag{16} F=SVD=S⎣⎡v1000v2000v3⎦⎤D(16)
其最小特征值 v 3 v_3 v3被设为0，以使得 F的秩为2。这样得到：
F = S V D = S [ v 1 0 0 0 v 2 0 0 0 0 ] D (17) F=SVD=S \begin{bmatrix} v_1 & 0 & 0 \\ 0 & v_2 & 0 \\ 0 & 0 & 0 \end{bmatrix}D \tag{17} F=SVD=S⎣⎡v1000v20000⎦⎤D(17)
上述 F F F就是最终得到的基础矩阵。

2.6 综上求解基础矩阵F

对上面进行总结，使用最小二乘法估算基础矩阵的步骤如下：

对取得两幅图像的匹配点进行归一化处理，转换矩阵分别是 T 1 , T 2 T_1,T_2 T1,T2，代码是FindFundamental

vector<cv::Point2f> vPn1, vPn2; // 两幅图像的匹配点
cv::Mat T1, T2;// 转换矩阵T1,T2
Normalize(mvKeys1,vPn1, T1);// 对图像一的匹配点进行归一化处理
Normalize(mvKeys2,vPn2, T2);// 对图像二的匹配点进行归一化处理

对应的匹配点( m ≥ 8 m≥8 m≥8)，构造系数矩阵A，代码是ComputeF21。

    cv::Mat A(N,9,CV_32F);for(int i=0; i<N; i++){const float u1 = vP1[i].x;const float v1 = vP1[i].y;const float u2 = vP2[i].x;const float v2 = vP2[i].y;A.at<float>(i,0) = u2*u1;A.at<float>(i,1) = u2*v1;A.at<float>(i,2) = u2;A.at<float>(i,3) = v2*u1;A.at<float>(i,4) = v2*v1;A.at<float>(i,5) = v2;A.at<float>(i,6) = u1;A.at<float>(i,7) = v1;A.at<float>(i,8) = 1;}

可以看到，代码正是对应着公式(6)

[ u 2 1 u 1 1 , u 2 1 v 1 1 , u 2 1 , v 2 1 u 1 1 , v 2 1 v 1 1 , v 2 1 , u 1 1 , v 1 1 , 1 u 2 2 u 1 2 , u 2 2 v 1 2 , u 2 2 , v 2 2 u 1 2 , v 2 2 v 1 2 , v 2 2 , u 1 2 , v 1 2 , 1 u 2 3 u 1 3 , u 2 3 v 1 3 , u 2 3 , v 2 3 u 1 3 , v 2 3 v 1 3 , v 2 3 , u 1 3 , v 1 3 , 1 ⋯ u 2 8 u 1 8 , u 2 8 v 1 8 , u 2 8 , v 2 8 u 1 8 , v 2 8 v 1 8 , v 2 8 , u 1 8 , v 1 8 , 1 ] ⋅ [ f 1 f 2 f 3 f 4 f 5 f 6 f 7 f 8 f 9 ] = 0 (6) \begin{bmatrix} u_2^1u_1^1,u_2^1v_1^1,u_2^1,v_2^1u_1^1,v_2^1v_1^1,v_2^1,u_1^1,v_1^1,1 \\ u_2^2u_1^2,u_2^2v_1^2,u_2^2,v_2^2u_1^2,v_2^2v_1^2,v_2^2,u_1^2,v_1^2,1 \\ u_2^3u_1^3,u_2^3v_1^3,u_2^3,v_2^3u_1^3,v_2^3v_1^3,v_2^3,u_1^3,v_1^3,1 \\ \cdots \\ u_2^8u_1^8,u_2^8v_1^8,u_2^8,v_2^8u_1^8,v_2^8v_1^8,v_2^8,u_1^8,v_1^8,1 \\ \end{bmatrix} \cdot \begin{bmatrix} f_1 \\ f_2 \\ f_3 \\ f_4 \\ f_5 \\ f_6 \\ f_7 \\ f_8 \\ f_9 \end{bmatrix}=0 \tag{6} ⎣⎢⎢⎢⎢⎡u21u11,u21v11,u21,v21u11,v21v11,v21,u11,v11,1u22u12,u22v12,u22,v22u12,v22v12,v22,u12,v12,1u23u13,u23v13,u23,v23u13,v23v13,v23,u13,v13,1⋯u28u18,u28v18,u28,v28u18,v28v18,v28,u18,v18,1⎦⎥⎥⎥⎥⎤⋅⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡f1f2f3f4f5f6f7f8f9⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=0(6)

对矩阵A进行SVD分解， A = U Σ V T A=U\Sigma V^T A=UΣVT。由向量 f = V 9 f=V_9 f=V9构造矩阵 F ^ \hat{F} F^
首先，是对A的SVD分解，公式是（14）：

A = U Σ V T (14) A=U\Sigma V^T\tag{14} A=UΣVT(14)

代码是：ComputeF21

cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);

然后，取V的最后一行列向量 V 9 V_9 V9作为解，因为上面分解过后，vt表示是V的转置矩阵 V T V^T VT，所以要转置回来，代码是：ComputeF21

cv::Mat Fpre = vt.row(8).reshape(0, 3);

对得到的矩阵 F ^ \hat{F} F^进行秩为2的约束，即对 F ^ \hat{F} F^进行SVD分解， F ^ = S ⋅ d i a g ( v 1 , v 2 , v 3 ) ⋅ V T \hat{F}=S⋅diag(v_1,v_2,v_3)⋅V^T F^=S⋅diag(v1,v2,v3)⋅VT，公式是（16），

F = S V D = S [ v 1 0 0 0 v 2 0 0 0 v 3 ] D (16) F=SVD=S \begin{bmatrix} v_1 & 0 & 0 \\ 0 & v_2 & 0 \\ 0 & 0 & v_3 \end{bmatrix}D \tag{16} F=SVD=S⎣⎡v1000v2000v3⎦⎤D(16)

代码是：TwoViewReconstruction.cc#L299

cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);

令 v 3 = 0 v_3=0 v3=0得到基础矩阵的估计 F ′ = S ⋅ d i a g ( v 1 , v 2 , 0 ) ⋅ V T F'=S⋅diag(v_1,v_2,0)⋅V^T F′=S⋅diag(v1,v2,0)⋅VT，公式是（17）：

F = S V D = S [ v 1 0 0 0 v 2 0 0 0 0 ] D (17) F=SVD=S \begin{bmatrix} v_1 & 0 & 0 \\ 0 & v_2 & 0 \\ 0 & 0 & 0 \end{bmatrix}D \tag{17} F=SVD=S⎣⎡v1000v20000⎦⎤D(17)

代码是：TwoViewReconstruction.cc#L301

cv::Mat Fpre = vt.row(8).reshape(0, 3);
cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
w.at<float>(2)=0;
return  u*cv::Mat::diag(w)*vt;

对上一步得到的解进行反归一化处理，得到基础矩阵 F = T 2 T F ′ T 1 F=T_2^TF'T_1 F=T2TF′T1，代码是：FindFundamental

F21i = T2t*Fn*T1;

2.7 Chi-square test(减少误差)

上面把F矩阵算出来之后，还没完，因为之前就说过了：

由于噪声、数值的舍入误差和错误的匹配点的影响，仅仅求解上面的线性方程组得到的基础矩阵非常的不稳定

我们的目的是要精确估算基础矩阵F，但是，不匹配的点能够造成很大的误差，即使是只有一对错误的匹配都能使估算值极大的偏离真实值。因此，需要找到一种方法，从包含错误点（外点）的匹配点对集合中，筛选出正确的匹配点（内点）。那么，除了之前讲的归一化方法，还有一个方法能减少错误匹配点的影响，那就是RANSAC，随机采样一致性算法，用这个例子说的话，可以理解成为，用这个方法来看看用我们得到的矩阵F推算出来的点，其误差的概率分布是否满足正态分布规律（大多数工程中，我们都会把误差的概率分布规律假设成正态分布，即高斯分布）。

补充一下，这里说的误差指的是随机误差，因为误差的概念不止随机误差一种，还有系统误差、粗大误差等。这里用的方法是利用概率法来消除随机误差。

我们很容易理解，在实践中，点对的匹配肯定是存在误差的，其中主要有两种类型的误差：

不精确的测量点位置引起的系统误差，通常服从高斯分布
错误匹配引起的误差，这些不匹配的点被称为外点，通常不服从高斯分布

再来看看我们面临的实际问题，现在我们利用很多对匹配点，按照八点法，通过求解齐次线性方程的最小二乘解得到基础矩阵F，如果我们可以计算出误差较大的点，并且把这些误差较大的点剔除掉，那么就可以大大提高基础矩阵F的精确度和可靠性。这样，我们不得不思考两个问题，误差怎么计算？怎么判断误差较大？

2.7.1 误差怎么计算？

来回顾一下，得到基础矩阵F的是什么原理？没错，是对极几何原理，就是前面章节《2-2-1 对极约束》的《三、基础矩阵Fundamental 代数推导》最后，也是《视觉十四讲》公式（7.9），表示为：
p 2 T F p 1 = 0 (18) p_2^TFp_1=0\tag{18} p2TFp1=0(18)

我们可以假设三维空间中的点 P 在 img1 和 img2 两图像上的投影分别为 p1 和 p2（两个为同名点），按照对极约束的原理，如果没有误差的情况下，即 e = 0 e=0 e=0时，表示投影点应该在极线上， p 2 p_2 p2一定存在于极线 l ′ l' l′上，即 p 2 ∗ l ′ = 0 p_2*l' = 0 p2∗l′=0。而 l ′ = F ∗ p 1 = ( a , b , c ) T l' = F*p_1 = (a, b, c)^T l′=F∗p1=(a,b,c)T。如果有误差，即存在 e ≠ 0 e\neq0 e=0时，表示投影点不在它应该在的位置，和事物规律不符，存在偏差。

这就很明显了，不在极线上的投影点的误差，可以用该点实际位置到极线的距离 d d d来表示，即
e = d i s t a n c e ( p → l ) (19) e=distance(p\rightarrow l)\tag{19} e=distance(p→l)(19)

看示意图（看不清的可以点击打开大图）：

2.7.2 怎么判断误差较大？

在这个章节开头就说了：

用得到的矩阵F推算出来的点，验证其误差的概率分布是否满足正态分布规律

我们到底用什么办法来验证这些数据的模型是否满足我们假设的模型呢？这就不得不感慨前人的智慧了，数学是博大精深的，任何现实的事物都逃不开数学公式的描述，特别随机误差，可以用概率与数理统计领域的知识来破解其中的谜题，而卡方检验方法正好满足了我们现在需要剔除误差较大点的需求。

2.7.3 什么是卡方检验？

用大白话说，卡方检验就是：

卡方检验就是检验两个变量之间有没有关系。

用通俗点的例子来描述，就是卡方检验可以解决比如：
检验硬币是否做了手脚？
男性或者女性对线上买生鲜食品有没有区别？
不同城市级别的消费者对买SUV车有没有什么区别？

这些都是实实在在的问题，我们可以通过大大大大大数据的分析来解决我们心中的疑问，但是，总感觉哪里不对。路人甲：“那个，我想问一下，卡方检验的对比数据是如何得来的？”路人乙：“就是在你安装和使用手机软件的时候得来的…”路人丙：“你咋那么多事捏？没听过一首歌吗？不要问我从哪里来～～我的故乡在远方～～”
专业点说，卡方检验是：

卡方检验就是统计样本的实际观测值与理论推断值之间的偏离程度

那么我们通过什么指标来衡量呢？那就是卡方值。

2.7.4 卡方怎么检验？

（在这里很想插句嘴，卡方检验的英文名叫chi-square test，而这个chi指的就是希腊字母 χ \chi χ，它的发音是chi）卡方检测过程：

简单来说，就是我们根据自由度，通过公式计算得到卡方值，然后通过卡方检验表获取我们的理论阈值，假如测量结果（实际结果）大于理论阈值，则假设不成立，反之就成立。

这里我会先讲2个容易误导思路的公式（20）和（22），同时也记录自己被误导的过程。（其实也不叫误导，我认为是概念上差异的差异而已，真正的思想都是利用统计学知识，将数据按照预先假设的概率密度函数（像高斯分布、泊松分布等）模型进行比对，从而进行有效评判）

2.7.4.1 误导一

要说清楚卡方检验非常复杂，感兴趣的童鞋可以自行搜索。在这里，我们只需搞清楚这几个概念：卡方值、自由度、卡方检验表。

卡方值

χ 2 = ∑ ( A − E ) 2 E (20) \chi^2= \sum \frac{(A-E)^2}{E} \tag{20} χ2=∑E(A−E)2(20)
其中， A A A为测量值（实际）， E E E为理论值。
下面这个式子描述得更专业（看不清的点击打开大图）：

自由度

计算E时要用到的参数个数

卡方检验表（看不清的点击打开大图）

看程序发现，实际上我们只会传入一个参数，就是mSigma，是TwoViewReconstruction类的成员变量float mSigma，它的含义是这些测量数据的标准差，默认值是1.0，float sigma = 1.0。而且在代码中，并没有利用上述公式进行计算，而是利用了标准差来计算，即chiSquare1 = squareDist1*invSigmaSquare

其中invSigmaSquare是标准差的平方分之一，即 1 σ 2 \frac{1}{\sigma^2} σ21，

代码对应的公式就是

χ 2 = d i s t a n c e ( p → l ) σ 2 (21) \chi^2=\frac{distance(p\rightarrow l)}{\sigma^2}\tag{21} χ2=σ2distance(p→l)(21)

和公式（20）

χ 2 = ∑ ( A − E ) 2 E (20) \chi^2= \sum \frac{(A-E)^2}{E} \tag{20} χ2=∑E(A−E)2(20)

相差甚远。为什么不一样呢？标准差和卡方分布之间，到底存在什么关系？

2.7.4.2 误导二

不啰嗦了，先上公式：

( n − 1 ) s 2 σ 2 ∼ χ 2 ( n − 1 ) (22) \frac{(n-1)s^2}{\sigma^2}\sim\chi^2(n-1)\tag{22} σ2(n−1)s2∼χ2(n−1)(22)

其中 s s s为样本的标准差。该式子的含义是 (n-1)×样本方差与总体方差之比服从自由度为n-1的卡方分布。而且， X ‾ \overline{X} X与 S 2 S^2 S2相互独立。

对比上面的公式（21），这个公式更接近我们代码中使用的形态了，但是还是不对啊，代码里分子用的是距离 d i s t a n c e ( p → l ) distance(p\rightarrow l) distance(p→l)，上面用的是样本方差 s 2 s^2 s2，有点风马牛不相及啊，让人摸不着头脑。

2.7.4.3 残差平方和与随机项方差的比值服从卡方分布

哈哈，好吧，不卖关子了，最后公式还是让我找对了，那就是和残差相关的卡方分布。上公式：

∑ ( x ^ i − x ‾ i ) 2 σ 2 ∼ χ 2 (23) \frac{\sum(\hat{x}_i-\overline x_i)^2}{\sigma^2}\sim\chi^2\tag{23} σ2∑(x^i−xi)2∼χ2(23)

什么意思呢？就是表示，残差平方和除以随机项方差的结果服从卡方分布。而在我们这个问题中，残差表示的是 p 2 T F 21 p 1 p_2^TF_{21}p_1 p2TF21p1，即匹配点到极线的距离。至于为什么，感兴趣的童鞋可以看看证明过程（证明残差平方和除随机项方差服从卡方分布）。

公式到底对不对？我们来看一下。

首先，搞清楚，残差的定义是什么？

残差平方和（RSS）是总和的的平方的残差（偏离实际经验值预测数据的），是数据与估计模型之间差异的一种度量，较小的RSS表示模型与数据紧密匹配。它被用作参数选择和模型选择的最优准则。

而我们这里要做什么？

我们通过直线线性转换方法中的SVD分解，得到齐次方程 p 2 T F p 1 = 0 (18) p_2^TFp_1=0\tag{18} p2TFp1=0(18)的最小二乘解。而结果 F F F带入方程中，结果是不等于零的，必然存在误差。为什么会存在误差？怎么理解我们提取出来的匹配点误差呢？

理想情况下是图像没有噪声污染与像素迁移与光线恒定，但是实际情况下图像特别容易受到光线、噪声导致像素迁移，从而产生额外的多余描述子匹配。(参考)

那么，结合上面残差的概念，我们要度量实际结果与估计模型之间的差异，回顾残差的概念：预测值和真实值之间的差值，很容易知道我们要做的就是让残差最小，说明我们实际结果和估计模型最接近。
但是，一个最直接的问题是，误差是无法被观测的，那么这样的话，要检验Gauss-Markov条件就会出现困难（参考）。统计学家在这个时候决定退而求其次，去对残差做一系列的分析，进而用它来去检验Gauss-Markov条件中，与误差有关的部分。所以，残差分析的一部分就是，计算出残差相关的统计量。

回到我们这个问题上，我们已知样本方差sigma（代码中的CheckFundamental输入参数之一），也知道某个点的残差（ p 2 T F p 1 p_2^TFp_1 p2TFp1），还有卡方分布原理，结合公式（23）和代码中的写法，我们容易知道，这个问题最终还是抽象成为这个公式（参考此过程）：

R S S σ 2 ∼ χ n − p 2 (23) \frac{RSS}{\sigma^2}\sim\chi_{n-p}^2\tag{23} σ2RSS∼χn−p2(23)

看一下RSS的定义：

很清楚的看到，这个问题里面，RSS是所有点的距离平方和。那既然所有点的距离平方和与随机项方差的比值服从卡方分布，那么对于单个点来说，一个点的距离的平方落在这个区域的概率应该是大于某个阈值的（结合自由度来判断这个阈值），而这就要用到卡方分布，来检验每个点是否符合模型的要求。

在此问题中，自由度为1（自由度可以理解为独立变量的个数，参考），阈值是3.841（const float th = 3.841;），如果大于阈值则是外点，否则是内点（if(chiSquare1>th) bIn = false;）。

这下问题搞清楚了，我们可以继续愉快地编程了。

2.8 中途小结

以上说了这么多，有点累，缓一缓，小结一下。

对极约束是原理基础，从物理世界出发描述了整个视觉相机成像、数据来源以及相互关系的根本问题，其中印象最深的是把搜索匹配点的范围缩小成一段极线，大大加速了匹配过程。
八点法从求解的角度出发，用公式描述了获得我们想要的解的最小条件，提供了有力的数学基础。
归一化使图像进行缩放，而缩放尺度是为了让噪声对于图像的影响在一个数量级上，从而减少噪声对图像的影响。
直接线性转换则从诸多的测量值中（超过8点的N个匹配点，超定方程）算出了最优的解，使我们基本得到了想要的解。
在已经有的粗解（自己想的，大概的意思是粗略未经过优化的，哈哈）基础上利用统计学方法进行分析，筛选出优质的点（符合概率模型的内点）来构成我们最终使用的单边解。

说到这，又有疑问了：什么叫单边解？问题还没解决完啊？当然没有，问题是源源不断的，怎么可能解决完呢？

2.9 重投影误差 & Chi-square

单边解只是我随意叫的，这里其实是指代码中，函数 CheckFundamental 明明有2个卡方阈值，但是上面我们只说了一个，另一个的值并没有在函数内部使用，而是通过结果输出到外部使用，究竟是怎么回事？

因为上面只讲了p1->p2的投影误差e1，还有p2->p1的投影误差e2没有讲。代码中的用法是将e1和e2看成是自由度为2的卡方检验，然后用（score += thScore - chiSquare1;）和（score += thScore - chiSquare2;）计算得到最终估计F矩阵模型效果的分数值（currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);），遍历得到效果最好的F矩阵。

利用单应矩阵H的时候也会计算重投影误差，反正就是利用极限约束，分别在两个图像进行投影，计算其投影点和实际点之间的差距，来评估模型的准确性。

2.10 恢复位姿（利用基础矩阵F）

《2 位姿估计》中说过了：

利用得到的最佳模型（选择得分较高的矩阵值，单应矩阵H或者基础矩阵F）估计两帧之间的位姿，代码中对应着函数ReconstructH或ReconstructF。其中，分两个步骤。第一是利用基础矩阵F和本质矩阵E的关系 F = K − T E K − 1 F=K^{-T}EK^{-1} F=K−TEK−1，计算出四组解。第二是调用的函数CheckRT作用是用R，t来对特征匹配点三角化，并根据三角化结果判断R,t的合法性。最终可以得到最优解的条件是位于相机前方的3D点个数最多并且三角化视差角必须大于最小视差角。

这里估计位姿的方法其实是SFM，正如《视觉十四讲》里面讲到，当我们没有测量运动的传感器，只有一张张的图像时，即只考虑观测方程带来的数据时，相当于估计 P (x|z) 的条件概率分布。如果忽略图像在时间上的联系,把它们看作一堆彼此没有关系的图片,该问题也称为 Structure from Motion(SfM)，即如何从许多图像中重建三维空间结构。

2.10.1 计算得到四组解

我们在利用本质矩阵E计算 R , t R,t R,t的时候，为什么会有四组解？《视觉十四讲》里面已经讲了：

其实上面公式（7.14）引用的是推理，（参考）：

以上结论都是大牛数学家和图像专家通过严谨的推理得出的结论，直接用就好了，如果有疑问或者感兴趣的话可以阅读《计算机视觉中的多视图几何》，里面绝对有你想要的答案。

利用上述结论，我们就对本质矩阵E进行SVD分解：

以上比较清晰的列出了公式的推导，知道了四组解的由来，那么结合代码，又发现了一个问题：

为什么代码中要对t做归一化处理？u.col(2).copyTo(t);t=t/cv::norm(t);

原因是：

尺度不确定性，用上面的方法估计出的相机平移向量t的值并没有单位，也就是说相机移动的距离只有相对值，没有绝对值。这是单目相机固有的尺度不确定性问题，无法从根本上解决。因此单目SLAM中一般把初始化后的t归一化，即把初始化时移动的距离默认为1，此后的距离都以这个1为单位。

而《视觉十四讲》也有说到：

由于 E 本身具有尺度等价性,它分解得到的 t, R 也有一个尺度等价性。而 R ∈ SO(3) 自身具有约束,所以我们认为 t 具有一个尺度。换言之，在分解过程中,对 t 乘以任意非零常数,分解都是成立的。因此,我们通常把 t 进行归一化，让它的长度等于 1。

这就是利用函数DecomposeE(E21,R1,R2,t);计算四组解的过程。

2.10.2 三角化

三角化定义：

三角化最早由高斯提出，并应用于测量学中。简单来讲就是：在不同的位置观测同一个三维点P(x, y, z)，已知在不同位置处观察到的三维点的二维投影点X1(x1, y1), X2(x2, y2)，利用三角关系，恢复出三维点的深度信息z。

已知量：

两帧图像中匹配到的特征点像素坐标 + 两帧之间的相机运动R(旋转)、T(平移)

未知量：

特征点三维空间坐标

通俗的讲，当有两个相对位置已知的相机同时拍摄到同一物体时，如何根据两幅图像中的信息估计出物体的实际位姿？

即通过三角化获得二维图像上对应点的三维结构

这正是三角测量要解决的问题。知道了要解决的问题，需要找到解决的办法。落实到公式上，是什么样子的呢？就是这个样子的：
s r x r = s l R x l + t (24) s_rx_r=s_lRx_l+t\tag{24} srxr=slRxl+t(24)
这个公式什么意思呢？ s l 、 s r s_l、s_r sl、sr分别为点 P P P在相机坐标系 O l 、 O r O_l、O_r Ol、Or中的z坐标值（即深度）， x l 、 x r x_l、x_r xl、xr分别为点 P P P在相机坐标系 O l 、 O r O_l、O_r Ol、Or中的图像坐标， R , t R,t R,t分别是旋转矩阵和平移矩阵。怎么得来的呢？具体看下面过程。

对公式（24）进行叉乘消元，可得：

s r x r ∧ x r = 0 = s l R x r ∧ x l + x r ∧ t (25) s_rx_r^∧x_r=0=s_lRx_r^∧x_l+x_r^∧t\tag{25} srxr∧xr=0=slRxr∧xl+xr∧t(25)

向量叉乘自身为零，所以根据上述公式右侧，因为 R , t , x r , x l R,t,x_r,x_l R,t,xr,xl都已知，所以很容易计算得到左侧点的深度值 s l s_l sl，然后也容易得到 s r s_r sr。

设 X X X为三维空间点在世界坐标系下的齐次坐标 X = [ x y z 1 ] X=\begin{bmatrix}x\\y\\z\\1\end{bmatrix} X=⎣⎢⎢⎡xyz1⎦⎥⎥⎤， T T T为世界坐标系到相机坐标系的变换 T = [ r 1 r 2 r 3 ] = K [ R t ] T=\begin{bmatrix}r_1\\r_2\\r_3\end{bmatrix}=K\begin{bmatrix}R &t\end{bmatrix} T=⎣⎡r1r2r3⎦⎤=K[Rt]， x x x为像素坐标 x = [ u v 1 ] x=\begin{bmatrix}u\\v\\1\end{bmatrix} x=⎣⎡uv1⎦⎤， λ \lambda λ为深度值。结合上面公式（9），有：

λ x = T X ⇒ λ x × T X = 0 x ^ T X = 0 (26) \lambda x=TX \\ \Rightarrow \lambda x \times TX=0 \\ \hat{x} TX=0 \tag{26} λx=TX⇒λx×TX=0x^TX=0(26)

其中， x ^ \hat{x} x^表示反对称矩阵，涉及到向量叉乘和矩阵转换的性质：

a × b = a ^ b = [ 0 − a 3 a 2 − a 3 0 − a 1 a 2 − a 1 0 ] [ b 1 b 2 b 3 ] (27) a\times b=\hat{a}b=\begin{bmatrix}0&-a_3&a_2\\-a_3&0&-a_1\\a_2&-a_1&0\end{bmatrix} \begin{bmatrix}b_1\\b_2\\b_3\end{bmatrix}\tag{27} a×b=a^b=⎣⎡0−a3a2−a30−a1a2−a10⎦⎤⎣⎡b1b2b3⎦⎤(27)

借助公式（26），由公式（27）得：

x ^ T X = [ 0 − 1 v 1 0 − u − v u 0 ] [ r 1 r 2 r 3 ] X ⇒ = [ − r 2 + v r 3 r 1 − u r 3 − v r 1 + u r 2 ] X (28) \hat{x} TX= \begin{bmatrix}0&-1&v\\1&0&-u\\-v&u&0\end{bmatrix} \begin{bmatrix}r_1\\r_2\\r_3\end{bmatrix}X \\ \Rightarrow = \begin{bmatrix}-r_2+vr_3\\r_1-ur_3\\-vr_1+ur_2\end{bmatrix}X \tag{28} x^TX=⎣⎡01−v−10uv−u0⎦⎤⎣⎡r1r2r3⎦⎤X⇒=⎣⎡−r2+vr3r1−ur3−vr1+ur2⎦⎤X(28)

其中，因为

[ − r 2 + v r 3 r 1 − u r 3 − v r 1 + u r 2 ] ⟹ − u [ u r 2 − u v r 3 r 1 − u r 3 − v r 1 + u r 2 ] ⟹ − v [ u r 2 − u v r 3 − v r 1 + v u r 3 − v r 1 + u r 2 ] (29) \begin{bmatrix}-r_2+vr_3\\r_1-ur_3\\-vr_1+ur_2\end{bmatrix} \stackrel{-u}{\Longrightarrow} \begin{bmatrix}ur_2-uvr_3\\r_1-ur_3\\-vr_1+ur_2\end{bmatrix} \stackrel{-v}{\Longrightarrow} \begin{bmatrix}ur_2-uvr_3\\-vr_1+vur_3\\-vr_1+ur_2\end{bmatrix} \tag{29} ⎣⎡−r2+vr3r1−ur3−vr1+ur2⎦⎤⟹−u⎣⎡ur2−uvr3r1−ur3−vr1+ur2⎦⎤⟹−v⎣⎡ur2−uvr3−vr1+vur3−vr1+ur2⎦⎤(29)

可知，第一行 × − u \times -u ×−u与第二行 × − v \times -v ×−v的和等于第三行，其线性相关，则保留第二行即可。有：

[ − r 2 + v r 3 r 1 − u r 3 ] X = 0 (30) \begin{bmatrix}-r_2+vr_3\\r_1-ur_3\end{bmatrix}X=0 \tag{30} [−r2+vr3r1−ur3]X=0(30)

因此，已知一个归一化平面坐标 x x x和变换矩阵 T c w T_{cw} Tcw，可以构建两个关于 X X X的线性方程组，有以上两个图像观测，即可求出 X X X：

[ − r 2 1 + v 1 r 3 1 r 1 1 − u 1 r 3 1 − r 2 2 + v 2 r 3 2 r 1 2 − u 2 r 3 2 ⋮ ] X = 0 (31) \begin{bmatrix} -r_2^{1}+v^{1}r_3^{1}\\r_1^{1}-u^{1}r_3^{1}\\ -r_2^{2}+v^{2}r_3^{2}\\r_1^{2}-u^{2}r_3^{2}\\ \vdots \end{bmatrix}X=0 \tag{31} ⎣⎢⎢⎢⎢⎢⎡−r21+v1r31r11−u1r31−r22+v2r32r12−u2r32⋮⎦⎥⎥⎥⎥⎥⎤X=0(31)

上诉方程没有非零解，使用SVD求最小二乘解，解可能不满足齐次坐标形式（第四个元素为1），因此：

X = [ x y z 1 ] = [ x 0 / x 3 x 1 / x 3 x 2 / x 3 x 3 / x 3 ] (32) X= \begin{bmatrix}x\\y\\z\\1\end{bmatrix}= \begin{bmatrix}x_0/x_3\\x_1/x_3\\x_2/x_3\\x_3/x_3\end{bmatrix} \tag{32} X=⎣⎢⎢⎡xyz1⎦⎥⎥⎤=⎣⎢⎢⎡x0/x3x1/x3x2/x3x3/x3⎦⎥⎥⎤(32)

这里再次复习一下SVD分解求最小二乘问题，之所以在代码中取的是右奇异值矩阵 V T V^T VT的最后一行（x3D = vt.row(3).t()），也就是右奇异值矩阵的 V V V的最后一列，是因为最小二乘解是最小奇异值 σ m i n \sigma_{min} σmin对应的列向量。证明过程：

2.10.3 源码

构造AX=0问题，计算矩阵A的时候，代码是这样的：

 //构造参数矩阵AA.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);

背后的公式是公式（31）的线性变换：

[ − r 2 + v r 3 − r 1 + u r 3 ] X = 0 (333) \begin{bmatrix}-r_2+vr_3\\-r_1+ur_3\end{bmatrix}X=0 \tag{333} [−r2+vr3−r1+ur3]X=0(333)

[ − r 2 1 + v 1 r 3 1 − r 1 1 + u 1 r 3 1 − r 2 2 + v 2 r 3 2 − r 1 2 + u 2 r 3 2 ⋮ ] X = 0 (34) \begin{bmatrix} -r_2^{1}+v^{1}r_3^{1}\\-r_1^{1}+u^{1}r_3^{1}\\ -r_2^{2}+v^{2}r_3^{2}\\-r_1^{2}+u^{2}r_3^{2}\\ \vdots \end{bmatrix}X=0 \tag{34} ⎣⎢⎢⎢⎢⎢⎡−r21+v1r31−r11+u1r31−r22+v2r32−r12+u2r32⋮⎦⎥⎥⎥⎥⎥⎤X=0(34)

利用SVD求的最小二乘解上面也提到了，代码中取的是右奇异值矩阵 V T V^T VT的最后一行：源码

x3D = vt.row(3).t();

也就是右奇异值矩阵的 V V V的最后一列，是因为最小二乘解是最小奇异值 σ m i n \sigma_{min} σmin对应的列向量。

最后进行归一化，满足齐次方程的解的要求：源码

x3D = x3D.rowRange(0,3)/x3D.at<float>(3);

五、总结

单目方案的初始化过程总结一下：

对极约束是原理基础，从物理世界出发描述了整个视觉相机成像、数据来源以及相互关系的根本问题，其中印象最深的是把搜索匹配点的范围缩小成一段极线，大大加速了匹配过程。
八点法从求解的角度出发，用公式描述了获得我们想要的解的最小条件，提供了有力的数学基础。
归一化使图像进行缩放，而缩放尺度是为了让噪声对于图像的影响在一个数量级上，从而减少噪声对图像的影响。
直接线性转换则从诸多的测量值中（超过8点的N个匹配点，超定方程）算出了最优的解，使我们基本得到了想要的解。
在已经有的粗解基础上利用统计学方法进行分析，筛选出优质的点（符合概率模型的内点）来构成我们最终使用的一个投影的最优解，利用两帧图像上匹配点对进行相互投影，综合判断内外点，从而最小化误差。
筛选出内外点之后，对两个模型进行打分，选出最优模型，然后通过三角化测量进行深度估计，最终完成初始化过程。

至此，单目的初始化过程的位姿估计（基于基础矩阵F）就完啦，最后一步地图的初始化等后续有时间再详细介绍一下，内容较多，希望不对的地方多多指教，相互学习，共同成长。以上仅是个人见解，如有纰漏望各位指出，谢谢。

参考：
1.对极几何及单应矩阵
2.2D-2D：对极约束
3.多视图几何
4.SVD分解及线性最小二乘问题
5.矩阵SVD分解（理论部分II——利用SVD求解最小二乘问题）
6.奇异值分解(SVD)原理详解及推导
7.最小二乘解（Least-squares Minimization ）
8.卡方检验（Chi-square test / Chi-square goodness-of-fit test)
9.样本标准差与自由度 n-1 卡方分布关系的证明
10.证明残差平方和除随机项方差服从卡方分布
11.本质矩阵优化分解的相对位姿估计
12.单目移动机器人的相对位姿估计方法
13.三角化求深度值（求三位坐标）

ORB-SLAM3 细读单目初始化过程（下）相关推荐

ORB-SLAM3 细读单目初始化过程（终结篇）
本文原创,转载请说明地址:https://blog.csdn.net/shanpenghui/article/details/110522368 一.前言请阅读本文之前最好把ORB-SLAM3的单目 ...
ORB-SLAM3 细读单目初始化过程(上)
学习ORB-SLAM3单目视觉SLAM中,发现有很多知识点需要展开和深入,同时又需要对系统有整体的认知,为了强化记忆,记录该系列笔记,为自己图方便,也希望对大家有所启发. TrackMonocular ...
超详细解读ORB-SLAM3 单目初始化过程（上篇）
学习ORB-SLAM3单目视觉SLAM中,发现有很多知识点需要展开和深入,同时又需要对系统有整体的认知,为了强化记忆,记录该系列笔记,为自己图方便,也希望对大家有所启发. 因为知识有限,因此先记录初始 ...
超详细解读ORB-SLAM3单目初始化（下篇）
一前言本文承接ORB-SLAM3 细读单目初始化过程(上),ORBSLAM3单目视觉有很多知识点需要展开和深入,初始化过程是必然要经历的,而网上资料不够系统,因此本文主旨是从代码实现出发,把初始化 ...
【ORB-SLAM2源码梳理6】Track()函数的第一步：单目初始化MonocularInitialization()
文章目录前言一.Track()函数二.单目初始化MonocularInitialization() 1. 判断单目初始化器是否创建,若没有就创建. 2. 已创建初始化器,判断特征点数目 3. 在 ...
ORB_SLAM2单目初始化策略
基本流程单目初始化程序存储在Initializer.cc中需要注意,对于双目/RGB-D相机,初始化时,由于可以直接获得相机的深度信息,因此无需求H/F,直接作为关键帧插入就行. 使用 ...
ORBSLAM源码理论分析2—单目初始化
ORBSLAM源码理论分析2-单目初始化 1.构造初始化帧1 2.第一次初始化 3.构造初始化帧2 4.F1与F2特征匹配 5.初始化解算位姿 5.1.计算单应矩阵 5.2.计算基础矩阵 5.3.评估 ...
单目初始化单应矩阵本质矩阵恢复R t 三角变换求 3D点
单目初始化单应矩阵本质矩阵恢复R t 三角变换求 3D点博文末尾支持二维码赞赏哦 ^_^ /* * This file is part of ORB-SLAM2 * * 单目相机初始化 * 用 ...
重磅直播|ORB-SLAM3经典单目初始化模块原理及实现
点击上方"计算机视觉工坊",选择"星标" 干货第一时间送达大家好,本公众号现已开启线上视频公开课,主讲人通过B站直播间,对3D视觉领域相关知识点进行讲解,并在 ...

ORB-SLAM3 细读单目初始化过程（下）