LVI-SAM

包含两个子模块：VIS(visual-inertial system)和LIS(lidar-inertial system)
视觉里程计通过最小化视觉和IMU测量的联合误差来获取。
LIS提取激光雷达特征并通过提取的特征与特征地图匹配来获得激光雷达里程计。特征地图以滑动窗口的方式维护。
状态估计问题被建模位一个最大后验概率(MAP)问题，使用iSAM2因子图优化囊括IMU预积分约束¹、视觉里程计约束、雷达里程计约束和闭环约束求解。

VIS

初始化

初始化的质量高度依赖初始位姿估计和IMU参数的准确性。
首先初始化LIS并获得系统状态估计x和IMU偏置b，然后根据图像时间戳插值关联到每个图像关键帧。
假设IMU偏置在两个图像关键帧之间是恒定的。

特征深度关联

在VIS初始化后，我们使用视觉里程计的结果将激光雷达帧配准到相机帧。
由于现代 3D 激光雷达经常产生稀疏扫描，我们堆叠多个激光雷达帧以获得密集的深度图。为了将特征与深度值相关联，我们首先将视觉特征和激光雷达深度点投影到以相机为中心的单位球体上。然后使用极坐标对深度点进行下采样并存储，以获得球面上的恒定密度。我们通过使用视觉特征的极坐标搜索二维K-D Tree来找到球面上距视觉特征点最近的三个深度点。视觉特征点的深度是视觉特征射线到笛卡尔空间中三个激光深度点形成的平面的长度。
通过检查三个最近深度点之间的距离来排除异常值。

故障检测

VIS在剧烈运动、光照变化和无纹理化境的情况下会出现故障。我们注意到，当VIS故障时会估计出较大的IMU偏置。
当跟踪特征点数量小于阈值或估计的IMU偏置大于阈值时，报告VIS故障。一旦检测到故障，VIS会重新初始化并通知LIS。

闭环检测

使用DBoW2词袋模型进行闭环检测，提取每一个新的图像关键帧的BRIEF描述符并将与之前起取的描述符进行匹配，DBoW2 返回的候选闭环图像的时间戳被发送到 LIS 进行进一步验证。

LIS

将当前激光雷达关键帧与世界坐标系下的局部特征地图匹配得到激光里程计约束。候选闭环约束首先由VIS提供，然后经过激光扫描匹配进行进一步优化。特征地图由一个包含激光雷达关键帧的滑动窗口维护。关键帧的选择标准是较前一帧的机器人位姿变化超过阈值。两个关键帧之间的激光雷达帧被丢弃，不用于因子图优化。

初始值猜测

初始值对于帧间匹配的成功起到至关重要的作用。LIS初始化前后的初值来源不同：

• 在LIS初始化前，认为机器人是由静止启动的，且IMU偏置和噪声为0，然后用两帧雷达扫描之间的IMU积分值作为帧间匹配的初值。LIS初始化成功后，估计IMU偏置和机器人的位姿和速度，传递给VIS帮助其初始化。

• 在LIS初始化之后，会有两个获取初始猜测的方法：

  1. 去偏后的IMU积分值
  2. VIS的输出

  VIS可用时使用VIS的输出作为初值，VIS故障时使用IMU积分值作为初值

故障检测

基于优化的状态估计问题的退化
帧间匹配名可以被建模为非线性最小二乘优化问题
$$\underset{T}{\min }\Vert AT-b{\Vert }^2$$
当$A^{T}A$的最小的特征值小于阈值时，认为LIS故障，在因子图优化时不考虑激光里程计约束。

ubuntu20.04运行lvi-sam

1. 将所有文件中的 #include <opencv/cv.h> 替换为#include <opencv2/opencv.hpp>
2. 在CMakeLists.txt 中将set(CMAKE_CXX_FLAGS "-std=c++11")替换为set(CMAKE_CXX_FLAGS "-std=c++14")

问题：

  error: ‘CV_RGB2GRAY’ was not declared in this scope
   53 |       cv::cvtColor(image, aux, CV_RGB2GRAY);

原因：
opencv<4中的CV_RGB2GRAY在opencv4中变为cv ::COLOR_RGB2GRAY

问题：

error: ‘CV_FONT_HERSHEY_SIMPLEX’ was not declared in this scope

原因：
opencv<4中的CV_FONT_HERSHEY_SIMPLEX 在opencv4中变为cv::FONT_HERSHEY_SIMPLEX

也可以在使用宏的地方加入#include<opencv2/imgproc/types_c.h>

---

1. On-Manifold Preintegration for Real-Time Visual–Inertial Odometry ↩︎