2021@SDUSC
2021年11月3日星期三——2021年11月5日星期五
一、学习背景：
在对LVI-SAM包下的视觉部分进行分析的过程中，我遇到了严重的困难。经过仔细地分析，我觉得是因为我对于视觉部分的理论基础了解的还不够详尽，因此有必要先停下脚步，阅读这部分的基础论文VINS-Mono来增强稳固自己的基础知识。
二、论文概要：
文章介绍了一种处理imu和摄像机组成的单目视觉惯性系统的数据的方法：VINS-Mono。
文章指出，这种方法利用了imu预积分和特征提取观测数据，基于紧耦合和非线性优化的方法获得了高精度的视觉惯性里程计。imu与视觉两者融合的好处在于：
视觉与IMU的融合可以借助IMU较高的采样频率，进而提高系统的输出频率。
视觉与IMU的融合可以提高视觉的鲁棒性，如视觉SLAM因为某些运动或场景出现的错误结果。
视觉与IMU的融合可以有效的消除IMU的积分漂移。
视觉与IMU的融合能够校正IMU的Bias。
单目与IMU的融合可以有效解决单目尺度不可观测的问题。
算法和论文的介绍主要分为数据的预处理，估计器的初始化，紧耦合、自标定、非线性优化的单目VIO的实现，对于紧耦合、回环检测重定位和全局位姿图的优化五个部分。
基于目前任务的要求，我阅读的重点部分放在视觉系统的初始化部分，也就是论文的第五节。
剩下的可以到以后再看。
初始化过程(SectV)提供了所有必要的值，包括姿态、速度、重力向量、陀螺仪偏置和三维特征位置，用于引导随后的基于非线性优化的VIO
但是，需要注意的是，我们分析的源代码是LVI-SAM，是VINS-Mono的升级版，因此在初始化时有雷达系统的参与，这里与我们参考的资料有所不同，分析代码时需要格外注意。
三、重点内容：V. ESTIMATOR INITIALIZATION
论文原文翻译：
出处：VINS-Mono翻译_Pancheng1的博客-CSDN博客_vins-mono
单目紧耦合VIO是一个高度非线性的系统.由于单目相机**（1）无法直接观测到尺度**，因此，如果没有良好的初始值，很难直接将这两种测量结果融合在一起。人们可以**（2）假设一个固定的初始条件来启动单目vins估计器**。然而，这种假设是不合适的，因为在实际应用中经常会遇到运动下的初始化.当IMU测量结果**（3）被大偏置破坏时**，情况就变得更加复杂了。事实上，初始化通常是单目vins最脆弱的步骤。需要一个健壮的初始化过程来确保系统的适用性。
说明：这三点就是初始化存在的意义。1是关键原因，2对于原因给出了更进一步的解释。3的原因是imu自己的偏移。
0、为什么需要初始化：
单目系统相比于双目系统不能够确定深度。这里深度的意思就类似于空间感。人类理论上单眼也是无法获得空间感的。但是大脑可以通过运动、透视、光源和常识来辅助处理获得信息，使得单眼的人也可以获得空间感。
同样的，相机获取的数据也可以通过处理转化为深度的图像，这种方法就是三角测量。
三角测量（Triangulation）又叫三角化，是根据前后两帧图像中匹配到的特征点像素坐标以及两帧之间的相机运动R、t，计算特征点三维空间坐标的一种算法。直观来讲，当有两个相对位置已知的相机同时拍摄到同一物体时，如何根据两幅图像中的信息估计出物体的实际位姿，即通过三角化获得二维图像上对应点的三维结构，这正是三角测量要解决的问题。
这里的具体原理牵扯到大量的数学运算，不再深究。
这里获取的深度并不是真实尺度下的深度（为什么？），还需要利用imu的信息进行处理。但是imu有旋转bias,
零偏(Bias)
当IMU保持静止时, 它依然会有一个很小输出，这个输出的数值就是零偏。它会受到IMU的上电状态、温度、内部结构等因素影响，比如陀螺仪理论上静止时应该是0°/s，实际上陀螺仪静止的输出是一个不为0的值。
而相机获得的视觉矩阵不存在bias，因此可以利用视觉来处理IMU的旋转bias。
世界真实的坐标系可以借助于g来获取。
需要指出的是，初始化工作只需要进行一次，因为尺度确定之后后续就不用再改变了，bias则会随着后续的优化进行实时的更新。（？怎么做到的）
我们采用松耦合的传感器融合方法得到初始值.我们发现，只有视觉的SLAM，或SfM，具有良好的初始化性质。在大多数情况下，视觉系统可以通过从**（1）相对运动方法**(如八点或五点算法或估计齐次矩阵)中导出初始值来引导自己。（2）通过将度量IMU预积分与目视SfM结果相匹配，我们可以粗略地恢复尺度、重力、速度，甚至偏置。这足以引导非线性单目vins估计器，如图4所示。
与在**（3）初始阶段同时估计陀螺仪和加速度计偏置的[17]相比，我们在初始阶段选择忽略加速度计偏置项**。加速度计偏置与重力耦合，由于重力向量相对于平台动力学的e大量级，以及初始阶段相对较短，这些偏置项很难被观测到。我们以前的工作对加速度计偏置标定进行了详细的分析。
说明：这里的相对运动方法和匹配恢复结果以及后边的估计偏差是论文接下来要做的事情。
A. Sliding Window Vision-Only SfM
初始化过程开始于一个只有视觉的SfM来估计一个高比例相机的姿势和特征位置的图表。
我们保持了一个帧的滑动窗口来限制计算复杂度。首先，我们检查了最新帧与之前所有帧之间的特征对应。如果我们能找到稳定的特征跟踪(超过30个跟踪特征)和足够的视差(20个以上的旋转补偿像素)在滑动窗口中的最新帧和任何其他帧之间，我们恢复相对旋转和这两个帧之间的上尺度平移使用五点算法。否则，我们会将最新的帧保存在窗口中，并等待新的帧。如果五点算法成功的话，我们可以任意设置标度，并对这两个帧中观察到的所有特征进行三角化。基于这些三角特征，采用透视n点(Pnp)方法来估计窗口中所有其他帧的姿态。最后，应用全局整束平差[36]将所有特征观测的总重投影误差降到最小。由于我们还没有任何世界帧的知识，我们设置了第一个相机帧(·)c0作为参考帧的SfM。所有帧的姿态(pckc0，qckc0)和特征位置表示相对于(·)c0。假设摄像机和IMU之间有一个粗糙测度的外部参数(Pcb,qcb)，我们可以将姿态从C帧转换到B帧(IMU)。
（1）其中s是匹配视觉结构与米制尺度的尺度参数，解决这个缩放参数是实现成功初始化的关键。
说明：这里就是上面所说的”尺度参数“
SFM的意思是Structure From Motion，
通过相机的移动来确定目标的空间和几何关系，是三维重建的一种常见方法。 它与Kinect这种3D摄像头最大的不同在于，它只需要普通的RGB摄像头即可，因此成本更低廉，且受环境约束较小， 在室内和室外均能使用。
这里借助了ros中的：
vi_sfm 是一个 ROS 包，用于从运动问题中解决视觉惯性结构。 该软件包在封闭形式的解决方案中融合了视觉和惯性数据。 封闭形式的解决方案由单个方程组成，该方程仅使用惯性测量单元 (IMU) 和在短时间间隔（~4 秒）内获取的单目相机特征，线性确定初始速度、到点特征的距离和重力 . 假设相机已校准，并且相机和 IMU 之间的转换已知并且可以使用 TF 转换来确定。
在文档中，这部分占据了较大的篇幅。参见initial_sfm。
坐标系:
w是世界坐标系(world frame)。重力方向与世界坐标系z轴对齐。
b是本体坐标系(body frame)，我们把它定义为与IMU坐标系相同。
c是相机坐标系(camera frame)。我们同时使用旋转矩阵R和Hamilton四元数q来表示旋转。我们主要在状态向量中使用四元数，也用旋转矩阵来表示三维向量的旋转。
ck表示获取第k个图像时的相机坐标系。⊗表示两个四元数之间的乘法运算。gw是世界坐标系上的重力向量。最后，我们将(^)表示为某一具体量的噪声测量值或估计值。五点算法：
在slam 和sfm领域，恢复相机位姿和3D点的坐标是其重要的任务，描述一个场景的3D点在不同相机的图像坐标之间的关系被称为对极几何关系。对极几何关系描述的矩阵通常有基本矩阵(fundamental matrix)、本质矩阵（essential matrix）、单应矩阵(homography matrix)。基本矩阵的求解算法有7点法、8点法；基本矩阵的求解算法有5点法、8点法；单应矩阵的求解算法为DLT算法。
全局整束平差：
可能是翻译错了，budle Adjustment 就是光束法平差？
重投影误差：指的真实三维空间点在图像平面上的投影（也就是图像上的像素点）和重投影（其实是用我们的计算值得到的虚拟的像素点）的差值，
因为种种原因计算得到的值和实际情况不会完全相符，也就是这个差值不可能恰好为0，此时也就需要将这些差值的和最小化获取最优的相机位姿参数及三维空间点的坐标。
pnp方法：
PnP(Perspective-n-Point)是求解 3D 到 2D 点对运动的方法。它描述了当我们知道n 个 3D 空间点以及它们的投影位置时,如何估计相机所在的位姿。
B. Visual-Inertial Alignment
1)陀螺仪偏置标定：考虑窗口连续两帧bk和bk+1，我们从视觉sfM中得到旋转qbkc0和qbk+1c0，以及通过IMU预积分得到的相对约束γbk+1bk。我们将IMU预积分项线性化，使陀螺仪偏置最小，并将下列成本函数降到最小：
其中B代表窗口中的所有帧。利用第四部分导出的偏置雅可比，给出了γbk+1bk对陀螺仪偏置的一阶近似。这样，我们得到了陀螺仪偏置bw的初始校准。然后我们用新的陀螺仪偏置重新传播所有的imu预积分项 。
2)速度、重力向量和米制尺度初始化：在陀螺仪偏置初始化后，我们继续初始化导航的其他基本状态，即速度、重力向量和公制标度：
其中，当取第k帧图像时，vbkbk是B帧中的速度，gc0是c0帧中的重力向量，s将单目SfM缩放为公制单位。
考虑窗口中两个连续的帧bk和bk+1，那么(5)可以写成：
我们可以将(14)和(17)合并成以下线性测量模型：
可以看出，Rbkc0 ,Rbk+1c0,Pckc0 ,Pck+1c0是从上尺度单目视觉∆tk中得到的，∆tk是两个连续帧之间的时间间隔。通过求解这个线性最小二乘问题：
我们可以得到窗口中每一帧的物体帧速度，视觉参照系(·)c0中的重力向量，以及尺度参数。
3)重力精化：通过约束量值，可以对原线性初始化步骤得到的重力向量进行细化。在大多数情况下，重力向量的大小是已知的。这导致重力向量只剩2*度。因此，我们在其切线空间上用两个变量重新参数化重力。参数化将重力向量表示为 代替(17)中的g，并与其它状态变量一起求解w1和w2。此过程迭代到g^收敛为止。
4)完成初始化：经过对重力向量的细化，通过将重力旋转到z轴上，得到世界帧与摄像机帧c0之间的旋转qc0W。然后我们将所有变量从参考框架(·)c0旋转到世界框架(·)w。B帧的速度也将被旋转到世界框架。视觉SfM的转换组件将被缩放为公制单位。此时，初始化过程已经完成，所有这些度量值都将被输入到一个紧耦合的单目VIO中。
说明：b部分的处理大都集中在对于数学原理和过程的详解中来，除去复杂的数学公式外，并没有太多的术语名词困扰我的理解。具体的原理还是暂时先不深究了。
四、问题及解答：
经过对于论文短期的探究，也算是大致弄明白了初始化的原理，但是这仅仅辅助工作，对于我来说，下一步就是要把这些知识和纸面上的内容回归到代码中去，弄清楚代码的相关原理，来解决整个包的问题。
五、参考资料：
VINS-Mono翻译_Pancheng1的博客-CSDN博客_vins-mono
多视图几何中5点法非线性求解计算本质矩阵_3D视觉工坊-CSDN博客
Bundle Adjustment—即最小化重投影误差（高翔slam—第七讲） - Jessica&jie - 博客园 (cnblogs.com)