烟草税下 -期刊分享-SLAM-VINSMono：一种稳健的单目视觉惯性状态估计器-机器学习4人工智能ML4AI
下 |期刊分享|SLAM|VINSMono：一种稳健的单目视觉惯性状态估计器-机器学习4人工智能ML4AI
编者序：本文提出的紧耦合滑窗VINS状态估计算法曹兴龙，可在未知状态情况下稳健初始化，具有线外参标定、统一定义在球面上的重投影误差、闭环检测和4自由度位姿图优化的特点。本文是香港科技大学沈邵劼课题组的开源杰作，媲美state-of-the-art。分为上、下两部分介绍，下部分侧重理论细节及应用。
推荐指数☆☆☆☆☆
一、总体框架回顾
本文提出的视觉惯性系统结构框图如下所示。第一部分是测量处理前端，负责提取并跟踪每个新图像帧的特征，及帧间IMU数据预积分。第二部分是初始化过程，提供了激活非线性系统的必要初始值，如姿态、速度、重力向量、陀螺仪偏差及3D特征位置。第三部分为非线性图优化，通过联合优化所有的视觉、惯性信息求解滑窗内的状态。另一部分运行另一个线程，负责闭环检测及姿态图优化。
二、测量预处理
2.1视觉预处理前端：
对每张新图像，使用KLT稀疏光流算法跟踪已有特征绝命速递 ，同时检测新的角点特征以维持每张图像100-300特征。检测器通过设置相邻特征最小间隔强制特征均匀分布，outlier去除后，特征投影在单位球面上（unit sphere）。Outlier通过RANSAC基础矩阵测试去除。
关键帧的选取原则有两个：平均视差和跟踪的特征数。当跟踪的特征数小于某个门限或者跟踪特征的平均视差超过某个门限时，插入关键帧。请记住，除了平移，旋转也会导致视差，但是纯旋转时特征无法三角定位，我们在计算视差时用IMU propagation结果补偿旋转。
2.2 IMU预积分
对于基于优化的算法来说，IMU预积分可节约大量的计算资源，因为不需要一遍遍计算IMU测量。IMU预积分部分，仅仅与IMU偏差有关粘人相公，当偏差变化小时，可通过一阶近似调整，否则才需要重新计算。离散时间预积分可通过Euler、Mid-point、RK4等数值计算方法计算。我们程序中采用内点法计算。
经推导整理，IMU测量模型可写成如下形式
三、初始化
3.1 滑动窗口视觉SfM
初始化时，需要先执行视觉SfM，估计up-to-scale相机位姿和特征位置。首先检查最后一帧和前面帧的特征匹配，如果找到多余30个跟踪特征并且平均视差对于20个像素的前面帧乌丸莲耶，则通过5点法恢复这两帧间的相对旋转和up-to-scale平移，否则将最后一帧留在滑动窗口并等待下一帧。如果5点法成功了，则固定平移的scale并三角化这两帧的所有特征。基于这些三角化的特征，使用PnP（Perspective-n-Point）算法n-Point估计窗口内其余帧的位姿。最后执行全局BA最小化所有特征的重投影误差。由于绝对尺度未知陈鸿锦 ，可用第一帧作为参考帧，其余帧的位姿和特征位置用第一帧表示。
3.2 视觉惯性对齐
首先根据SfM估计出的相邻两帧间的旋转和IMU预积分得到的约束，通过将IMU预积分线性化为陀螺仪偏差的函数，通过最小化窗口内所有帧的旋转误差；计算出陀螺仪偏差。然后构建并求解最小二乘问题得到速度、重力向量和尺度的初始估计；进而通过约束重力向量的幅度常高技，在tangent空间求解出重力向量精确值（尤其是重力方向），如下图所示。最后根据旋转矩阵调整速度和尺度花手纸，完成初始化古墓倩影。
四、紧耦合非线性优化
4.1 公式
优化公式中包含窗口内的所有关键帧（参数有平移、旋转、速度、加速度偏差和陀螺仪偏差）和所有特征（特征的inverse depth）。优化的损失函数是边缘化的先验信息和视觉与IMU误差的Mahalanobis范数之和，如下所示。
其中IMU剩余误差和视觉剩余误差依次s如下图所示。
我们在边缘化IMU状态和图像特征的同时，根据边缘化状态将测量加入到先验信息。边缘化策略如下图所示。当倒数第二帧是关键帧时坦特尼克号 ，将最早一帧及其相应测量边缘化；如果倒数第二帧不是关键帧，我们仅仅将倒数第二帧的视觉测量扔掉而保留它的IMU测量。该策略增加了系统的稀疏性。边缘化不仅可以保持窗口内的关键帧数量恒定俾鬼捉，而且限定了预积分IMU测量的不确定性。我们使用Schur消元实现边缘化，通过边缘化，去掉状态的重要信息得以保留且计算复杂度恒定（important information of the removed states is kept and computationcomplexity is bounded）。
由于光照强烈变化或剧烈运动，失败不可避免。主动监测错误并从错误中恢复可以增加系统的实用性杀价帮。错误检测的原则有：跟踪的特征数或最新一帧特征数少于5、最后两个输出的位置或旋转有突变、偏差或外参估计有很大的值。检测到错误后徐公子胜治 ，以最后可能的位姿，重新初始化系统。同时，保留关键帧数据库，用于闭环检测。
五、闭环检测
5.1 闭环检测
我们使用DBoW2词袋模型和BRIEF描述子检测闭环。当新插入关键帧时，视觉前端提取的特征并不够，需要额外提取500个FAST角点特征，并计算所有特征的BRIEF描述子。检测到闭环时傅经年，执行时空一致性检查后，DBoW2返回候选关键帧，将原始图像扔掉而保留BRIEF描述子，以节省内存。
5.2 特征检索
当检测到闭环时袁郡梅，烟草税需要检索特征建立连接，本文仅建立含深度信息的前端提取的特征间的连接。直接匹配会有很多outlier，我们限定了匹配区域，每个特征仅在对应位置邻域内搜索。然后使用RANSAC检测几何一致性移除outlier。当匹配的特征数超过一定门限时，认为是正确的闭环匹配，融合二者信息西来古镇 ，如下图所示。
5.3 重定位
由于新帧和窗口内的其它关键帧有很多相同的特征，因此很容易将闭环帧与窗口内关键帧进行联合优化，加入闭环帧仅增加了视觉约束而没有增加IMU约束，如下所示八珍汤配方。
5.4 位姿图加入关键帧
当关键帧被边缘化时，将它加入位姿图猝不及防造句，实际上为减少计算量每三个才加入一个。这些关键帧是位姿图的顶点，位姿图的边分为顺序边和闭环边两种咱俩结婚吧 。
5.5 4DOF位姿图优化
由于惯性视觉系统可以观测到绝对尺度、俯仰角和滚动角，累积漂移仅出现在位置xyz和偏航角4个自由度上。因此闭环后，需要进行4自由度而不是6或7自由度位姿图优化脚镯吧。其中闭环边的优化函数选huber稳健损失函数，如下所示。
5.6 数据库管理
随着轨迹的增加，数据库变得越来越大，闭环检测和位姿图优化时间越来越长，长时间实时应用困难。我们限定数据库大小恒定侯马588，当关键帧数量超过某个门限时
，根据关键帧密度分布去掉一些密集的关键帧，从而避免上述问题。
六 应用
无人机平台及IPhone7手机上实验结果如下图所示。其中手机平台是与tango做对比，无人机平台用的是鱼眼相机。
英文名称：VINS-Mono:A Robust and Versatile Monocular Visual-Inertial State Estimator
链接及源码：https://github.com/HKUST-Aerial-Robotics/VINS-Mono/blob/master/support_files/paper/tro_technical_report.pdf
死磕自律，遇见更好的自己；自斩双臂矮人矿坑，方能长出强两翼。
败而不弃，潜龙勿用待时机；执着坚毅，飞升上神创奇迹！
关注该公众号，一起创造奇迹。
编者序：本文提出的紧耦合滑窗VINS状态估计算法曹兴龙，可在未知状态情况下稳健初始化，具有线外参标定、统一定义在球面上的重投影误差、闭环检测和4自由度位姿图优化的特点。本文是香港科技大学沈邵劼课题组的开源杰作，媲美state-of-the-art。分为上、下两部分介绍，下部分侧重理论细节及应用。
推荐指数☆☆☆☆☆
一、总体框架回顾
本文提出的视觉惯性系统结构框图如下所示。第一部分是测量处理前端，负责提取并跟踪每个新图像帧的特征，及帧间IMU数据预积分。第二部分是初始化过程，提供了激活非线性系统的必要初始值，如姿态、速度、重力向量、陀螺仪偏差及3D特征位置。第三部分为非线性图优化，通过联合优化所有的视觉、惯性信息求解滑窗内的状态。另一部分运行另一个线程，负责闭环检测及姿态图优化。
二、测量预处理
2.1视觉预处理前端：
对每张新图像，使用KLT稀疏光流算法跟踪已有特征绝命速递 ，同时检测新的角点特征以维持每张图像100-300特征。检测器通过设置相邻特征最小间隔强制特征均匀分布，outlier去除后，特征投影在单位球面上（unit sphere）。Outlier通过RANSAC基础矩阵测试去除。
关键帧的选取原则有两个：平均视差和跟踪的特征数。当跟踪的特征数小于某个门限或者跟踪特征的平均视差超过某个门限时，插入关键帧。请记住，除了平移，旋转也会导致视差，但是纯旋转时特征无法三角定位，我们在计算视差时用IMU propagation结果补偿旋转。
2.2 IMU预积分
对于基于优化的算法来说，IMU预积分可节约大量的计算资源，因为不需要一遍遍计算IMU测量。IMU预积分部分，仅仅与IMU偏差有关粘人相公，当偏差变化小时，可通过一阶近似调整，否则才需要重新计算。离散时间预积分可通过Euler、Mid-point、RK4等数值计算方法计算。我们程序中采用内点法计算。
经推导整理，IMU测量模型可写成如下形式
三、初始化
3.1 滑动窗口视觉SfM
初始化时，需要先执行视觉SfM，估计up-to-scale相机位姿和特征位置。首先检查最后一帧和前面帧的特征匹配，如果找到多余30个跟踪特征并且平均视差对于20个像素的前面帧乌丸莲耶，则通过5点法恢复这两帧间的相对旋转和up-to-scale平移，否则将最后一帧留在滑动窗口并等待下一帧。如果5点法成功了，则固定平移的scale并三角化这两帧的所有特征。基于这些三角化的特征，使用PnP（Perspective-n-Point）算法n-Point估计窗口内其余帧的位姿。最后执行全局BA最小化所有特征的重投影误差。由于绝对尺度未知陈鸿锦 ，可用第一帧作为参考帧，其余帧的位姿和特征位置用第一帧表示。
3.2 视觉惯性对齐
首先根据SfM估计出的相邻两帧间的旋转和IMU预积分得到的约束，通过将IMU预积分线性化为陀螺仪偏差的函数，通过最小化窗口内所有帧的旋转误差；计算出陀螺仪偏差。然后构建并求解最小二乘问题得到速度、重力向量和尺度的初始估计；进而通过约束重力向量的幅度常高技，在tangent空间求解出重力向量精确值（尤其是重力方向），如下图所示。最后根据旋转矩阵调整速度和尺度花手纸，完成初始化古墓倩影。
四、紧耦合非线性优化
4.1 公式
优化公式中包含窗口内的所有关键帧（参数有平移、旋转、速度、加速度偏差和陀螺仪偏差）和所有特征（特征的inverse depth）。优化的损失函数是边缘化的先验信息和视觉与IMU误差的Mahalanobis范数之和，如下所示。
其中IMU剩余误差和视觉剩余误差依次s如下图所示。
我们在边缘化IMU状态和图像特征的同时，根据边缘化状态将测量加入到先验信息。边缘化策略如下图所示。当倒数第二帧是关键帧时坦特尼克号 ，将最早一帧及其相应测量边缘化；如果倒数第二帧不是关键帧，我们仅仅将倒数第二帧的视觉测量扔掉而保留它的IMU测量。该策略增加了系统的稀疏性。边缘化不仅可以保持窗口内的关键帧数量恒定俾鬼捉，而且限定了预积分IMU测量的不确定性。我们使用Schur消元实现边缘化，通过边缘化，去掉状态的重要信息得以保留且计算复杂度恒定（important information of the removed states is kept and computationcomplexity is bounded）。
由于光照强烈变化或剧烈运动，失败不可避免。主动监测错误并从错误中恢复可以增加系统的实用性杀价帮。错误检测的原则有：跟踪的特征数或最新一帧特征数少于5、最后两个输出的位置或旋转有突变、偏差或外参估计有很大的值。检测到错误后徐公子胜治 ，以最后可能的位姿，重新初始化系统。同时，保留关键帧数据库，用于闭环检测。
五、闭环检测
5.1 闭环检测
我们使用DBoW2词袋模型和BRIEF描述子检测闭环。当新插入关键帧时，视觉前端提取的特征并不够，需要额外提取500个FAST角点特征，并计算所有特征的BRIEF描述子。检测到闭环时傅经年，执行时空一致性检查后，DBoW2返回候选关键帧，将原始图像扔掉而保留BRIEF描述子，以节省内存。
5.2 特征检索
当检测到闭环时袁郡梅，烟草税需要检索特征建立连接，本文仅建立含深度信息的前端提取的特征间的连接。直接匹配会有很多outlier，我们限定了匹配区域，每个特征仅在对应位置邻域内搜索。然后使用RANSAC检测几何一致性移除outlier。当匹配的特征数超过一定门限时，认为是正确的闭环匹配，融合二者信息西来古镇 ，如下图所示。
5.3 重定位
由于新帧和窗口内的其它关键帧有很多相同的特征，因此很容易将闭环帧与窗口内关键帧进行联合优化，加入闭环帧仅增加了视觉约束而没有增加IMU约束，如下所示八珍汤配方。
5.4 位姿图加入关键帧
当关键帧被边缘化时，将它加入位姿图猝不及防造句，实际上为减少计算量每三个才加入一个。这些关键帧是位姿图的顶点，位姿图的边分为顺序边和闭环边两种咱俩结婚吧 。
5.5 4DOF位姿图优化
由于惯性视觉系统可以观测到绝对尺度、俯仰角和滚动角，累积漂移仅出现在位置xyz和偏航角4个自由度上。因此闭环后，需要进行4自由度而不是6或7自由度位姿图优化脚镯吧。其中闭环边的优化函数选huber稳健损失函数，如下所示。
5.6 数据库管理
随着轨迹的增加，数据库变得越来越大，闭环检测和位姿图优化时间越来越长，长时间实时应用困难。我们限定数据库大小恒定侯马588，当关键帧数量超过某个门限时
，根据关键帧密度分布去掉一些密集的关键帧，从而避免上述问题。
六 应用
无人机平台及IPhone7手机上实验结果如下图所示。其中手机平台是与tango做对比，无人机平台用的是鱼眼相机。
英文名称：VINS-Mono:A Robust and Versatile Monocular Visual-Inertial State Estimator
链接及源码：https://github.com/HKUST-Aerial-Robotics/VINS-Mono/blob/master/support_files/paper/tro_technical_report.pdf
死磕自律，遇见更好的自己；自斩双臂矮人矿坑，方能长出强两翼。
败而不弃，潜龙勿用待时机；执着坚毅，飞升上神创奇迹！
关注该公众号，一起创造奇迹。