基础矩阵/单应矩阵/三角化的实际应用
目的
通收获如下内容：
- 对极几何、对极约束以及基础矩阵（Fundamental）/本质矩阵（Essential）的数学原理，并且明白为何视觉SLAM中需要对极几何
- 单应矩阵（Homography）的基础概念，以及单应矩阵与基础矩阵的区别
- 如何通过匹配点对求解基础矩阵和单应矩阵
- 如何通过基础矩阵和单应矩阵求解相机的位姿
- 三角化的基础概念和应用
- ORBSLAM中的地图初始化详细流程
- 一个精简的剥离了复杂的ORB_SLAM3框架的地图初始化代码，并进行详细的中文注释。同时，提供可直接运行的demo。
本文代码：https://github.com/zeal-up/ORB_SLAM_Tracking
Part1 视觉SLAM中的地图初始化
在视觉SLAM中，地图初始化是一个非常重要的步骤，它决定了后续的跟踪、建图等模块的效果。简单来说，初始化的目的是利用前几帧图像，计算出相机位姿，并且构建出第一批3D地图点
第一批3D地图点为跟踪、建图等模块提供了一个初始的地图，这样后续的模块就可以利用这些地图点进行跟踪、建图等工作。
由于ORB_SLAM系列工作将传感器模式扩展到了：单目、双目、RGBD、IMU+单目、IMU+双目、IMU+RGBD等多种模式，所以地图初始化的方式也有所不同。
双目由于已知两个相机的内外参，可以直接三角化出3D点。三角化是单目的一个步骤，所以双目初始化比较简单。RGBD可以直接通过深度值还原3D坐标，更加简单。 配置了IMU之后实际也可以直接两个相机之间的姿态，也可以直接三角化出3D点（虽然IMU初始化需要考虑IMU参数的初始化，但是这部分内容不在本文的讨论范围内）。
因此，本文将重点放在单目初始化上。单目初始化由于涉及到对极几何和三角化和矩阵求解等知识，对于初学者会比较复杂。
Part2 ORB_SLAM地图初始化流程
01-init_pipeline
本文将对这流程中的每一个步骤进行详细的讲解，并且给出代码实现。
Part3 ORB特征提取及匹配
特征点提取是一个比较独立的内容，对于ORB特征提取可以参考这篇文章：https://zeal-up.github.io/2023/05/18/orbslam/orbslam3-ORBextractor/
Part4 对极几何及对极约束
4.1从实际问题出发
为什么视觉SLAM需要对极几何？
02-feature_point_matching
那么，我们现在的问题是，如何从这些点对的匹配关系，得到相机之间的相对位姿？
PS：通常，我们会将第一帧图像当作参考帧，也就是世界坐标系。第二帧相机的位姿也就是相对于第一帧相机的位姿。
4.2 对极几何
所谓的对极几何，就是指物体与两个相机之间空间几何关系。我们可以利用这个几何关系来求解相机之间的相对位姿。
4.3 对极约束
4.4 基础矩阵（Fundamental Matrix）F与本质矩阵（Essential Matrix）E
公式（7）和公式（9）都可以叫做对极约束。公式（7）忽略了相机内参的影响，因为一般在SLAM中相机内参是已知的，而归一化平面坐标可以方便地由相机内参和像素坐标得到，因此公式（7）是公式（9）的简化形式。
为了简便，我们可以记公式（9）中间部分为两个矩阵
要注意的是，由于上式中等式右边是0,因此左右两边同时乘以一个系数等式依旧成立，这一点我们在后面会再次提到
4.5 对极约束的几何意义
4.6 本质矩阵与基础矩阵的求解
4.6.1 基础矩阵的求解推导
本质矩阵是一个3x3矩阵，但其是由旋转与平移组成（6自由度），并且，要注意到公式（11）可以对任意尺度成立，也就是说本质矩阵可以放缩任意一个尺度，因此，本质矩阵有5个自由度。 因此，从理论上最少可以由5对点对求解（注意这里与PnP算法的不同，PnP算法等号两边没有0,一个点对可以建立两个等式，因此一个点对可以约束2个自由度。而这里一个点对只能建立一条等式，也就是约束一个自由度，因此需要5个点对）
04-solve_essential_matrix
以上方程求解后就可以得到本质矩阵（对任意尺度成立）
4.6.2 使用OpenCV求解
OpenCV有Api可以直接通过匹配点对计算基础矩阵
Mat cv::findFundamentalMat(
InputArray points1,
InputArray points2,
int method,
double ransacReprojThreshold,
double confidence,
int maxIters,
OutputArray mask = noArray()
)具体说明可以参考Api说明：findFundamentalMat()
Part5 单目尺度不确定性
5.1 如何理解单目尺度不确定性
让我们再次回到对极几何的图
Part6 单应矩阵（Homography Matrix）
我们前面讨论基础矩阵的概念以及如何从一些匹配点对关系中求解基础矩阵。我们没有对关键点是否在一个平面上进行任何假设。但是，如果我们假设关键点在一个平面上，那么我们就可以使用单应矩阵（Homography Matrix）来求解相机之间的位姿。
05-homography_example
当匹配点对的关键点都是在3D空间中一个平面上时，这些点对关系可以通过单应矩阵来描述（相差一个常量系数）
06_homographyMatrix
6.1 单应矩阵与相机内外参的关系
6.2 单应矩阵的求解
6.2.1 单应矩阵的求解推导
6.2.2 使用OpenCV求解
OpenCV提供接口可以直接求解单应矩阵
Mat cv::findHomography(
InputArray srcPoints,
InputArray dstPoints,
int method = 0,
double ransacReprojThreshold = 3,
OutputArray mask = noArray(),
const int maxIters = 2000,
const double confidence = 0.995
)详细的使用可以参考Api说明：cv::findHomography()
如果对内部实现有兴趣，可以参考这篇文章博客：对OpenCV::findHomography的源码解读
Part7 从H、E矩阵恢复R、t
7.1 基础要点
F矩阵与E矩阵只相差相机内参，通常我们会将F矩阵转换为E矩阵，然后再恢复R、t。
从H、E矩阵求解R、t涉及到比较复杂的数学推导，对于CV从业人员我认为这部分可以忽略具体推导过程，但是要抓住几个要点：
7.2 从E矩阵恢复R、t
求解推导过程忽略，我们只需要知道根据E可以分解出两对R,t如下
08-four_situation_of_E
7.3 使用OpenCV从E中求解R,t
OpenCV提供了一个api可以直接从E矩阵中分解出R、t
void cv::decomposeEssentialMat(
InputArray E,
OutputArray R1,
OutputArray R2,
OutputArray t
)
详细使用参考API说明：cv::decomposeEssentialMat
7.4 从H矩阵恢复R、t
从H矩阵恢复R、t有多种方法，论文中的方法叫Faugeras SVD-based decomposition，最终可以求解出8种解。另外一种有名的数值解法（通过奇异值分解）叫Zhang SVD-based decomposition
OpenCV的接口使用的是分析解法：https://inria.hal.science/inria-00174036v3/documentOpenCV的接口最终返回4种解
OpenCV的接口使用说明：cv::decomposeHomographyMat
Part8 如何选择正确的R、t
无论从E矩阵还是H矩阵中恢复出R、t，都会得到多种解。我们需要从这些解中选择出正确的解。 最简单的做法是利用分解出的R、t对匹配点进行三角化，并检查该3D点在两个相机下的深度值，3D点必须在两个相机下都是正的才是正确的解。
对于单应矩阵的分解结果，OpenCV提供了一个函数可以帮助我们选择正确的解：cv::filterHomographyDecompByVisibleRefpoints
在ORB_SLAM中，对于每一种解，都会对所有匹配点进行三角化，对三角化出来的点，会进行很多步骤的检查，最后选择拥有最多内点的解作为正确的解。
Part9 三角化
9.1 三角化的基础概念和推导
9.2 三角化的求解实现
可以用SVD求解上述方程，求解出的3D坐标有4个元素，需要将第四个元素归一化为1。这里把ORB_SLAM的这部分代码也贴出来
/**
* @brief 三角化获得三维点
* @param x_c1 点在关键帧1下的归一化坐标
* @param x_c2 点在关键帧2下的归一化坐标
* @param Tc1w 关键帧1投影矩阵 [K*R | K*t]
* @param Tc2w 关键帧2投影矩阵 [K*R | K*t]
* @param x3D 三维点坐标，作为结果输出
*/
bool GeometricTools::Triangulate(
Eigen::Vector3f &x_c1, Eigen::Vector3f &x_c2, Eigen::Matrix<float,3,4> &Tc1w, Eigen::Matrix<float,3,4> &Tc2w,
Eigen::Vector3f &x3D)
{
Eigen::Matrix4f A;
// x = a*P*X， 左右两面乘x的反对称矩阵 a*[x]^ * P * X = 0 ，[x]^*P构成了A矩阵，中间涉及一个尺度a，因为都是归一化平面，但右面是0所以直接可以约掉不影响最后的尺度
// 0 -1 v P(0) -P.row(1) + v*P.row(2)
// 1 0 -u * P(1) = P.row(0) - u*P.row(2)
// -v u 0 P(2) u*P.row(1) - v*P.row(0)
// 发现上述矩阵线性相关(第一行乘以-u加上第二行乘以-v等于第三行），所以取前两维，两个点构成了4行的矩阵（X分别投影到相机1和相机2），就是如下的操作，求出的是4维的结果[X,Y,Z,A]，所以需要除以最后一维使之为1，就成了[X,Y,Z,1]这种齐次形式
A.block<1,4>(0,0) = x_c1(0) * Tc1w.block<1,4>(2,0) - Tc1w.block<1,4>(0,0);
A.block<1,4>(1,0) = x_c1(1) * Tc1w.block<1,4>(2,0) - Tc1w.block<1,4>(1,0);
A.block<1,4>(2,0) = x_c2(0) * Tc2w.block<1,4>(2,0) - Tc2w.block<1,4>(0,0);
A.block<1,4>(3,0) = x_c2(1) * Tc2w.block<1,4>(2,0) - Tc2w.block<1,4>(1,0);
// 解方程 AX=0
Eigen::JacobiSVD<Eigen::Matrix4f> svd(A, Eigen::ComputeFullV);
Eigen::Vector4f x3Dh = svd.matrixV().col(3);
if(x3Dh(3)==0)
return false;
// Euclidean coordinates
x3D = x3Dh.head(3)/x3Dh(3);
return true;
}9.3 使用OpenCV进行三角化
OpenCV也提供接口进行三角化，且可以批量操作：cv::triangulatePoints
Part10 ORB_SLAM中的初始化
ORB_SLAM中对于初始化部分的操作虽然基础原理离不开上面的内容，但是具体实现上有很多细节，不仅仅反应作者的深厚算法功底，也反映了作者的工程能力。我们这里介绍ORB_SLAM中初始化部分的一些特殊操作
10.1 自动选择Homography或者Fundamental模型进行初始化
10.2 从H、F矩阵恢复R、t
选择了H或者F模型之后，会对H/F进行分解得到R、t。对于每一种[R,t]组合，会对所有匹配点对进行三角化，然后对三角化出来的3D点进行检查，去除以下情况的点：
- 深度为负（需满足在两个相机下都是正的）
- 重投影误差大于某个阈值（默认为2个像素）
同时，对每个点会统计其到两个相机光心的夹角，最后对所有夹角进行从小到大排序，选择第50个夹角，如果这个夹角小于1度，会认为初始化失败。
10.3 只在特征金字塔的最底层进行特征点匹配
在ORB_SLAM中，初始化阶段特征点匹配是在特征金字塔的最底层进行的。这一点也可以理解。图像金字塔主要是用来解决移动造成的尺度变化，而在初始化阶段，两帧图像不会有大的尺度变化，因此只需要在最底层进行匹配即可。
Part11附录A：在复现过程中的一些问题
PS：本文提供详细注释的精简代码，并附有demo：https://github.com/zeal-up/ORB_SLAM_Tracking
A-1 H矩阵的分数一直比F模型的分数低
在实验的过程中发现，H矩阵的分数一直比F模型的分数低，初始化时一直是选择的F模型进行初始化。后来发现ORBSLAM3中有一个bug,在使用H模型重建时并没有对3D点赋值，所以H模型其实可能一直没有被用上。https://github.com/UZ-SLAMLab/ORB_SLAM3/pull/806
A-2 与OpenCV求解H/F矩阵的对比
笔者在使用ORB_SLAM提供的代码进行初始化的时候很容易失败，而且每次运行的结果可能都不太相同。后来尝试使用OpenCV的接口求解H/F矩阵，发现每次运行的结果都比较一致，而且很少失败。OpenCV在求解H/F矩阵时也是用的RANSAC方法，区别比较大的应该是分数的计算。ORB_SLAM中计算分数的时候是计算双边投影误差，而OpenCV中计算分数的时候是计算内点个数。这两种方法的区别可能导致了ORB_SLAM中的实现对随机数非常敏感，每次运行的结果都不一样。
同时，使用ORB_SLAM的实现找出来的H/F矩阵，很容易在后续的三角化阶段被拒绝。而使用OpenCV接口求解的H/F矩阵，很少被拒绝。
另外，使用OpenCV的接口计算H、F矩阵，速度要比ORB_SLAM中的实现快很多，一个原因可能还是双边投影误差的计算需要两次投影。
Part12 附录B：Demo结果
PS：本文提供详细注释的精简代码，并附有demo：https://github.com/zeal-up/ORB_SLAM_Tracking
B.1 特征提取结果
同时可视化特征点的角度方向
run_01-features
B.2 特征点匹配
run_02-matches
B.3 成功三角化的特征点
run_03-matchesWithFinestFeatures
B.4 三角化的点
