留个笔记自用

LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping

做什么

Monocular Visual Odometry单目视觉里程计

相机在运动过程中连续两帧之间会存在overlap，即会同时观测到三维世界中的某些场景以及特征点。而这些场景特征点会投射到2D图片上，通过图片的对齐或者特征的匹配，可以找到前后图片上特点或patch的对应关系。利用相机的成像几何模型（包括相机参数）以及约束，可以求出两帧之间的运动信息（旋转矩阵R和平移t）。这样我们就可以得到一系列的相机相对变化矩阵，从而可以推出相机的姿态信息。
Lidar Odometry激光雷达里程计，里程计作为移动机器人相对定位的有效传感器，为机器人提供了实时的位姿信息。移动机器人里程计模型决定于移动机器人结构和运动方式，即移动机器人运动学模型。
简单来说，里程计是一种利用从移动传感器获得的数据来估计物体位置随时间的变化而改变的方法

用建图的方式来理解，要实现机器人的定位与导航，就需要知道机器人走了多远，往哪走，也就是初始位姿和终点位姿，只有知道了里程计，才能准确将机器人扫描出来的数据进行构建。

做了什么

该框架提出了一种通过平滑和映射实现激光和视觉里程计紧耦合的框架，VIS利用LIS估计来促进初始化，通过使用激光雷达测量提取视觉特征的深度信息，提高了可见光的精度。反过来，LIS利用初始猜测的可见性估计来支持扫描匹配
简单来说，这里做的就是一个lidar+camera的跨模态里程计

怎么做

整体框架由两个部分组成，第一个部分Visual-Inertial System（VIS视觉部分），第二个部分Lidar-Inertial System（LIS雷达部分），这也是设计的主旨，整体的输入是帧图像、帧点云还有一个惯性测量单元IMU
视觉部分的输入是帧图像和IMU，再加以雷达系统的辅助，通过最小化视觉和IMU的联合残差得到里程计。雷达部分的输入是帧蒂娜云，提取点云特征，然后将点特征和滑动窗口的局部特征图进行匹配得到里程计。最后组合优化融合。
然后是具体的部分，首先是VIS视觉部分

具体的结构设计借鉴了论文《VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator》

步骤大致是这样的，首先用角点检测器检测视觉特征，然后用Kanade–Lucas–Tomasi算法追踪，在初始化状态的基础上用里程计注册激光雷达帧，并且获得一张视觉深度图。
定义每帧状态为x

这里的R是SO(3)也就是旋转矩阵R∈R^3×3，p是三维坐标向量，v是速度，b是由b_a和b_w组成的IMU惯性偏置值，一个是加速度a偏置，一个是角速度w的偏置
定义位姿转换T∈SE(3)，也就是前面的R和和平移向t∈R³组成的位姿转换
然后是具体的步骤
首先是视觉特征计算，这里采用的方法前面有介绍，具体的文章内也没有细说，理解上就是得到了视觉图像帧的点特征。
然后是初始化也就是前面的第一步
这里采用的方法是结合LIS框架进行初始化，首先初始化LIS，就能获得这里的状态x和b，然后根据图像时间戳差值到图像关键帧里去，然后这些结果被视作VIS的初始化状态，至此得到了帧状态
然后是第二步，特征深度关联
主要目的就是得到深度图，而这个深度图只能用激光雷达数据得到，这里采用的方法是堆叠多个激光雷达帧来获得，然后就是将特征与深度进行关联，首先将视觉特征和深度点均投影到一个球平面上，这个球平面是以相机为中心的单位球，然后，深度点被下采样，并计算它们的极坐标，再通过视觉特征投影的极坐标找到最近邻的深度投影点三个，然后根据这三个点构成的三角形中心与图像点的连接线长度视为图像点的深度点。
至此，得到了图像帧上图像点的深度，这个深度信息是由激光雷达给出的，做法也就是上面的图a，右边的图b是对这个深度估计的检测，这是通过滑动窗口的局部时间得到验证的

上图是可视化效果，也就是图像帧上逐点所关联的深度信息
最后是对这张深度结果的故障检测，当被跟踪特征的数量低于阈值时，或当估计的惯性测量单元偏差超过阈值时，重新进行特征初始化
然后是第二部分，LIS部分

具体结构借鉴了论文《LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping》

主要是利用因子图来做全局的位姿优化，也设计了四种约束，IMU的预积分约束，视觉里程计的约束，雷达里程计的约束和闭环检测的约束，这些均在前面那片LIO-SAM中有
然后是具体的步骤
第一步仍然是LIS的初始化，这里的初始化还决定了VIS，因为VIS的初始化是基于LIS的，所以这里也很重要，这里的方法是假设初始速度为0，假设偏差和噪声为零，对原始惯性测量单元测量值进行积分，然后平移和旋转矩阵通过初始化雷达关键帧匹配猜测得到。这个初始化猜测则是通过VIS得到。
然后一样也是故障检测

这里的A和b是T的线性化得到的，方法是去计算A^TA的特征值，特征值小于阈值的时候就报故障。

最后再回到这张图看下整体，camera输入图片，通过特征提取和雷达深度图得到视觉深度图，这个深度图在LIS帮助初始化的odometry模块下进行里程计计算。idar输入点云，通过特征提取计算一张深度图给VIS，再用特征进行匹配的全局优化。

总结

1.没看过VIS和LIS基于的两盘文章，属实有点难以理解这是在讲什么，但是可以借鉴的是框架的设计模式，lidar和image的交互方式可以借鉴