基于滤波的VIO：MSCKF ：通过imu修正重投影误差来校准位姿
基于优化的VIO：VINS ： 通过预积分及重投影对位姿进行约束
一、ROS
1、ROS优点
-
分布式进程：以节点为最小单位进行编程，通过rosmasyer管理所有node
-
配套的开发套件：rviz，rosbag
-
开放的功能包
2、VINS-Mono中的ros
-
每个功能包中都有一个.xml文件，定义功能包运行时间及依赖
-
VINS mono 中的功能包：
camera_model 相机离线标定
benchmark_publisher 发布帧值，进行实时比较
vins_estimator 后端滑窗、优化
feature_tracker 前端光流
pose_graph 回环检测
-
节点就是ros运行的基本单元，可以理解成一个进程，一个main函数
查看每个功能包的cmakelist中有几个可执行文件，就代表有几个节点。
-
节点及消息通讯
-
Launch文件：可以同时启动多个节点，否则一个节点占用一个终端
可以配置参数文件
在VINS中可以通过添加要启动的节点实现一次启动多个命令，例如添加
<include file="$(find vins_estimator)/launch/vins_rviz.launch"/>
二、坐标系的定义
坐标系有右手和左手系。大多数SLAM是基于右手系。
相机坐标系：y向下，z向前，x向右。
IMU坐标系：x向前，y向左，z向上。
每个点都在某个坐标系下，slam很重要的坐标系叫做世界系。一般把SLAM的第一帧认为是世界原点。
变换矩阵一般记作
符号一般用
通常向量会记一下在哪个系下的向量。比如 
表示在w系下。
三、滤波和优化
-
置信度的概念
-
卡尔曼滤波公式
两个模型（状态转移模型和观测模型）
两个假设（线性矩阵和高斯分布模型（噪声））
本质上是一个加权平均的过程
-
扩展卡尔曼滤波（EKF）（解决卡尔曼滤波处于非线性状态的情况）
在估计点处利用一阶导的方式进行线性近似
-
滤波只对当前最佳状态进行估计，优化是根据过去一段时间的所有观测求出过去一段时间的最大后验估计
-
最大似然估计本质上是曲线拟合
-
处理优化问题时用核函数使outlier点的影响尽量小，其他时候采用的方法还有ransac
-
keyframe：仅优化关键帧位姿，例如选取接触到GPS的点作为关键帧
-
imu预积分：将很多imu数据转化为一个观测，提供帧间约束。GPS全局约束
-
在非线性优化中，x是待估计量，f（x）是残差，目标是让残差尽量小。在视觉中残差指的是重投影误差，imu中指的是两帧之间pose的变化量
四、Mono前端
1、feature_tracker前端光流节点
输出三个消息
/feature_tracker/feature_Img 用于rviz可视化
/feature_tracker/feature 向后端提供特征点信息
/feature_tracker/restart 重启节点
2、 特征点信息
-
通过特征点提取算法获取像素坐标
-
通过特征点去畸变算法获取去畸变后归一化坐标
-
通过光流追踪算法获取特征点id
-
获得特征点速度
3、 光流追踪
1、在光流的领域中，有一个一个基本假设，“相邻两帧灰度不变”。
在一个图里找一个特征点，这个特征点附近的一块区域的灰度，和另一个图里一个区域的灰度要尽量 相同。
2、图像金字塔（提升光流追踪的稳定性）
-
当像素追踪最优解的距离与原像素过远时，容易陷入局部最小问题
-
通过图像金字塔，可以提升光流追踪的稳定性。通过图像金字塔的缩放，可以拉进2个图里像素的距离，从而提升光流追踪的稳定性。
-
我们把上一层金字塔的结果，作为一下层金字塔追踪的初值，一直追踪到Level0。图上是把一次的光流追踪，变为了多次光流追踪，准确度提升了，但是花费更多的时间。并不是金字塔越多越好，但是花费时间多，所以要权衡时间开销和精度的需求。
4、 img_callback
主要包涵有3个部分，
1、通过时间戳来判断光流追踪是否成功。
2、控制给后端发送信息的频率，控制不要超过10Hz，给后端节约运算资源。
注意，这里被舍弃的帧还是需要做光流追踪的工作。因为光流需要假设2帧之间的变化特别小。
3、图像均衡化
如果失败的话，就要模块重启。
这个函数里工程上有一些技巧：
1、需要要check自己的一些特征相关工作是否是正确运行。
2、后端不一定要处理所有的数据，避免后端处理不过来。这里需要用一定的策略来实现调度。
5、readImage函数
大致的思路是做图像预处理，然后光流追踪，提取新的特征点，最后特征点去畸变，计算速度。首先是判断是否图像均衡化，如果图像太暗或者太亮，提取特征点会比较难。那么就需要提升图像对比度，来提取角点。这里直接用的是opencv的函数createCLAHE来实现均衡化。特别要注意，在这个函数里，这里prev是上一次的图像，forw是当前图像的意思。用上一帧的特征点消息，就可以和当前帧进行光流追踪了。随后调用opencv的函数calcOpticalFlowPyrLK直接光流追踪。
6、去畸变
-
简单的说，不考虑畸变的世界3D点投到2D相机平面，就是相机的K矩阵乘以世界点坐标，随后把xy坐标除以z，就得到了相机坐标了。
-
将x和y轴除以z轴，z值为1就是像素归一化
-
VINS去畸变方法，逐次逼近式去畸变方法
五、IMU预积分
-
IMU预积分文件夹：vins_estimator->src->intergration_base.h
六、Mono VIO初始化
1、初始化
-
VIO初始化文件夹：vins_estimator->src->
-
VINS是基于优化的VIO，非常依赖初始值。优化问题，经常用迭代下降的方式，初始值不好的话，会陷入局部最优解（或者不是我们想要的部分极小值）。 为了保证系统的极小值是我们要的，那么初始值就需要距离正确值比较近。这样迭代次数也少一些，也节约了计算时间和运算资源，也可以保证实时性。但是初始化很难给到完全接近的装状态，所以这里采用松耦合的方式。
-
初始化变量：位姿、零偏、地图点、外参；不初始化变量：加速度计零偏、平移外参
-
确定关键帧：比orb简单一些，判断两帧之间的视差，两个图像里面出现明显超过阈值的像素误差，则作为关键帧。
2、后端优化预处理
-
readParameters
读参数用，通过ROS参数服务器获取配置文件。这些参数是yaml格式的。 这里要设置单次优化的最大求解时间，最大迭代次数，以及根据视差确定关键帧。VINS判断关键帧比较简单，就是判断2帧之间的视差。同一个特征点，在2个图像里像素变化太大了，就视为关键帧。IMU噪声和随机游走的的参数，都是提前获得的。如果外参足够精确，是提前给定的，那么就不需要优化外参了。如果完全不知道外参，那么就认为外参是单位矩阵。后端优化还有一个功能，估计传感器和相机之间的时间误差问题。对于视觉，也肯定需要重投影误差的准确度的衡量。这里是通过虚拟相机，统一设置成1.5个像素的误差。
-
predict
算2个时间戳，算IMU之间的时间差。提取加速度，角速度，去掉bias和重力影响。根据上一个时刻，IMU在世界坐标系下的位姿，根据2个时刻IMU数据的平均值，视为这一小段时间的IMU的位姿在世界的变化量。 这时候，就可以算出最新时刻的IMU在世界的位姿，以及当前的速度。
-
getMeasurement
相机和IMU时间对齐
这一步操作的主要原因，是IMU和相机可能时间并不完全同步。 IMU数据通常要密集一些，但是还是不会很巧合的和相机数据的时间完全一致，这样的话，我们就算出一个虚假的IMU（通过各种方法）插入到和图像对齐的时间位置上。
在这里，我们需要保证最后的IMU时间要大于图像帧最早的时间。如果IMU最新的时间比图像还要早，那么就需要等待新的IMU数据。 如果IMU最早的时间戳，也显然需要小于时间的时间戳。这样的话，就扔掉图像，再等一个新的图像。 保证了每个图像的时间被2个IMU夹着，就可以进行对齐操作了。
-
processIMU
主要功能是更新预积分量，执行预积分的操作。其次功能是是给优化变量提供初始值(利用IMU进行状态推算)， 这里会整个代码是在一个死循环里。首先用时间对齐后的图像和IMU的数值。遍历每一组匹配好的IMU和图像的数据。整理好所有对齐好的数据，调用processImage。
-
processImage
对特征点信息进行预处理。
list中每一个元素都是一个id的特征点
起始帧id：在滑窗中第几帧被看到
深度： 该id特征点在起始帧下的深度
求解状态：是否被正确三角化
每帧中的属性：每个id特征点会被多帧看到，用vector来维护
把特征点加入特征点管理器，并判断是否是关键帧。VINS是对每个特征点进行编号，用一个list维护。每一个元素包涵id，起始帧，深度，求解状态和属性。 起始帧就是从第几帧看到这个特征。深度是在起始帧下的深度。求解状态是是否被正确的三角化出来。一个ID可能被多帧看到，所以他还有在其他帧的属性。这里要记录他在其他帧中的像素坐标，归一化相机坐系坐标以及特征点速度，就需要维护上述的信息。
在addFeatureCheckParallax函数中，主要是检查上一帧是否时关键帧， 如何判断一个帧是关键帧？如果他是第一次出现的帧，肯定是关键帧。如果当前帧追踪到的点过少，也认为上一帧就是关键帧。判断关键帧主要看倒数第二帧，和倒数第三帧是否视差大，如果视差大，那么也可以视为关键帧。如果得到了关键帧，那么就需要把滑动窗口里最早的一帧给扔掉。如果视差不够的话，并且窗口满了的话，那么倒数第二帧就需要被弹出了。判断完当前图片是否是关键帧后，处理好滑动窗口队列，并整理数据。如果某一帧被删除，滑窗需要中将两个预积分（约束）合并为一个，但是在初始化中则需要保留。
-
后端优化预处理总结
estimator_node.cpp->main
-
收到imu的值->buffer->imu_callback->以imu的频率向外发送里程计（通过发布imu预测位姿，提高里程计发布频率与imu相同）
-
前端光流结果->buffer->feature_callback->processImage
-
光流结果与imu时间戳同步
-
维护特征点（见processImage介绍）->关键帧判断（视差）->边缘化相关关键帧->预积分结果+光流结果->all_image_frame
3、旋转外参初始化
-
getCorresponding
这个函数是得到2帧看到的特征点，并计算他们在各自帧下的坐标是什么样子。 首先遍历所有的featrue，要确保这个feature同时被查询的2个帧的时间都能对的上。 然后把他们在2帧下归一化相机坐标系的坐标捆绑起来，整个打包发出来。这就是上述函数的主要功能和实现。
-
CalibrationExRotation
初始化旋转外参。首先用之前介绍的返回内的内容，直接计算出2帧的旋转。
-
solveRelativeR（计算两帧的旋转）
输入就是两个图像之间被绑定好关系的特征点对 (注意是点对)。 首先需要保证要有9对以上的匹配。（匹配点多，也会精度高）调用opencv的函数findFundamentalMat函数，得到本质矩阵（代码里用E表示）。 随后对矩阵E进行分解就可以得到其中的旋转部分。本质矩阵分解有4种不同的结果（十四讲里也有说），随后我们check这4个解哪一个可以用。其行列式的值必须为1。这里还用了测试三角化的方式进行检查( 函数testTriangulation)。 已知道Rt的变化量，以及知道两帧的点对关系，利用opencv的函数trianglePoints实现三角化（得到三角化后的点云）。根据三角化点云的坐标，我们可以肯定这些点的深度在两帧里都是大于0的，我们统计深度大于0的点占所有点的比例。比例越大，这样的求解是正确的可能性越大。这样我们就利用图片得到了2帧之间的旋转了。
-
构建A矩阵
可能求解出很多相邻帧之间的旋转，把他们都用一个vector保存起来。类似的，我们还有很多相邻帧之间IMU积出的旋转，用另外一个vector保存起来（当然，这里要注意一下IMU坐标系和相机坐标系的转换）。此时构建A矩阵所需的材料已经足够了，代码里，根据误差的度数来降低旋转误差超过5度权重。构造L和R矩阵，就利用四元数乘法转化矩阵乘法的方式进行转化。注意代码里是虚部在前，实部在后表达四元数。注意最后要每一行还要乘上权重。
-
求解外参
代码里用的是现成的SVD分解进行求解。代码里多了一个判断
-
要积累足够的旋转匹配对（匹配多，可信度更高）
-
检查倒数第二个奇异值要大于0.25。如果有几个奇异值都约等于0的话，那么可能数值不太稳定。这意味着整个系统的旋转太少。最后，如果CalibrationExRotation函数返回了true，我们就得到了的旋转外参值。
七、单目视觉位姿估计
当满足数据帧足够多时，同时没有初始化失败的话这两个条件后，就可以进行单目SLAM了。 目的是求解滑窗里的位姿和3D点。整个过程一共分为四步:
首先check一下IMU的能观性，希望IMU得到足够的激励。
第二步是简单的SFM，简单的三维重建。这一步只针对滑窗里的东西
第三步是利用滑窗的位姿，对所有帧进行恢复。恢复完后，就进行视觉，惯性的对齐。
第四步：视觉重建的结果和IMU结果进行对齐。
-
check IMU的能观性
这一步我们求解IMU在一段时间里加速度的方差，如果方差较大，说明IMU得到了充分的激励（动的挺多）。方差较小可能IMU没怎么动。作者的代码里最终并没有区分两种情况，只是写在了这里。可能作者经过实验发现这个影响较小，所以作者注释掉了这段代码。
-
全局单目SLAM
首先遍历特征点管理器的特征点。对每一个特征点，我们都默认他没有被三角化成功，并且保存特征点在每一帧中的性质。VINS用一个vector保存所有的特征点。首先假设一共11帧数据，我们首先要选择一个“枢纽帧”。我们希望枢纽帧尽可能距离最后一帧比较远，如果比较近的话，平移就比较小，这种情况下分解出来的移动接近于0，这样不太好，但是不希望太远，因为太远的话，两帧的关联就比较少（能关联的特征点比较少），这样也不好。在保证匹配点足够多的情况下，距离最后一帧尽可能的远。假设选择了第四帧，如图所示
假设他的旋转是单位矩阵，平移是0，通过对极几何求解出4和10之间的相对位姿态（R和t，但是t尺度未知）。这样我们就得到了第4和第10帧的位姿。 此时我们可以三角化出很多点的坐标。有了更多被三角化后的点，我们就可以用pnp的方式求解出第5-第9帧的位姿。此时我们可以三角化出更多的点（比如在第五帧，我们又有了更多的特征点，从而可以三角化出更多的点），求解完枢纽帧到最后一帧之间的位姿后，前几帧的方式也用类似的方法即可得到他们的位姿。
3. relativePose 寻找枢纽帧的函数
首先求解枢纽帧和最后一帧的Rt。 穷举每一帧到最后一帧的匹配对，如果匹配对超过20个匹配，则可以计算2帧的平均视差，得大于30个像素，看看这帧是否符合要求，随后求解2帧的Rt。
求解Rt的函数是solveRelativeRT,这里依旧是调用opencv的函数求解本质矩阵E。得到E后，直接用opencv的函数cv::recoverPose 分解出R和t。这里要求判断求解E的内点的数量，必须大于12个才认为是可行的解。得到Rt后，就有了枢纽帧到最后一帧的关系了。下一个任务就是通过枢纽帧和最后一帧，求解这之间其他帧的Rt，这些事情全部都是在sfm.construct函数里完成。
4. sfm.construct
这里注意一个细节，通常视觉slam维护的是Tcw，但是vins维护的是Twc，求解的是frame0和frame1之间的frame的三角化，遍历特征点，并且判断特征点是否已经三角化过。如果没三角化过的点才需要三角化，这些特征点需要同时被frame0和frame1看到，才进行三角化操作，只不过这三角化中，额外增加了一个观测的信息，这里三角化没有调用opencv的接口，是作者自己写了一个过程。
5.solveFrameBvPnp
这是通过pnp求解其他帧的过程。 主要内容依旧是遍历所有特征点，但是只使用三角化成功的特征点。同时还要检查这些三角化后的点，看他们是否能被当前要做PNP的这一帧看到。 我们还希望能用的匹配点尽量多一些，太少则认为匹配失败。这里的PNP求解是直接调用opencv的接口进行PNP求解。特别的，这里用上一帧的位姿作为初始值进行求解。此时函数帧的位姿就可被求解出来了。
现在有了新的帧的位姿，可以去和最后一帧进行三角化，从而三角化出更多的点，来维持整个系统继续工作下去。
根据上面的图，我们可以把4-10帧的位姿都求解出来，考虑到三角化的策略是用当前帧和最后帧进行三角化，但是部分特征点不能被最后一帧看到的特征点，就不能被三角化出来。因此还需要把这些不能三角化的特征点，去和枢纽帧进行三角化。此时0-3帧的数据还没有被求解，因为3距离4最接近，我们尽量把能和枢纽帧匹配的特征点给三角化出来。比如先求解第三帧的位置，随后再求2，求1最后求第0帧。最终求出整个滑窗中的位姿和一堆三角化后的点。我们依旧有一些点没有被三角化，我们遍历所有的特征点，用他们最初和最后一帧的位姿进行三角化。总之就是想尽办法把所有点都三角化出来。
此时我们三角化的并不太准，因为我们只是用2帧进行简单的三角化。为了让结果更加精确，我们还是做一个全局BA，我们之前的工作，就是给BA一个初始值而已。
八、基于ceres自动求导
平移是没办法得到尺度的，单目slam是没有真实坐标的。这也是我们亟待解决的问题。 vins是利用ceres进行优化求解的。ceres有多种求导方式，数值、解析和自动求导，其中自动求导就不需要人工进行雅克比矩阵的求解了。在初始化问题中，我们要优化的状态量x，包括滑窗中11帧的位姿，和一堆三角化的点，通过优化使得重投影误差最小
图中，蓝色的星，是3D点的位置，圆球是相机的位置。理论上，我们可以求出五角星在相机的像素平面上和我们检测的像素有多大的像素差（蓝色平面上，2个五角星之间的距离）。不管是调整3D点，还是调整相机位置，尽量使重投影误差最小。
ceres的优点是我们只需要定义残差，就可以自动计算了。缺点也很明显，是自动求导是需要时间的，肯定不如直接给出雅克比矩阵的解析要快。解析求导需要手动求，大量手动求导的赋值工作很影响代码的美观，初始化的部分对实时性要求略低，就不需要解析解了。正常来说，迭代优化问题需要有“变化量“作为步长，但是四元数是没有”加法“的，ceres帮我们写好了关于四元数相关的功能，所以使用ceres来进行优化很省功夫。这里要注意，枢纽帧的旋转不参与优化的，最后一帧的平移也不参与优化。这样整个系统的尺度就不会发生变化了。经过优化，如果求解成功，就得到了滑窗中所有的信息了。如果求解失败，则丢弃最小帧，准备下一次求解。有了关键帧后，就可以恢复出其他普通非关键帧的位姿了。我们遍历这些普通帧，如普通帧的时间戳和滑窗一样，则说明他是关键帧。对于关键帧，就直接得到了这个帧的位姿态，记录下即可。如果不是关键帧，那么就用PNP求解位姿即可。
扫一扫
970
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
