Towards High-Performance Solid-State-LiDAR-Inertial Odometry and Mapping
IN2LAAMA: Inertial Lidar Localization Autocalibration and Mapping. 用预积分去畸变，可以参考
系统架构
- 10hz激光雷达
- Xsens MTi-670 200Hz imu
- 首先使用陀螺仪对点云去畸变（旋转）
- 提取平面、边缘线特征
- 基于点-线、点-面距离，进行scan-matching，求出相对运动后进一步对点云去畸变（平移）
- 选择关键帧策略
- 滑动窗口优化
融合窗口通常包含了几个关键帧，当窗口滑动时，将对窗口内的关键帧状态进行优化，
其中,
- \(\breve{x}_k , \breve{q}_k\)表示关键帧位姿
- \(\breve{v}_k\)表示速度
- \(\breve{b}_k = [\breve{b}_{k,a}^T,\breve{b}_{k,g}^T]^T\)表示imu的bias
最终，维护一个全局的位姿图，对所有的lidar关键帧位姿进行整合
特征点提取
这里关于Livox激光雷达的特征提取
提出出来的边缘线特征和平面特征对前端里程计以后后端滑动窗口都有用
为了量化传感器融合过程中每个LiDAR残差的贡献，根据关联质量提出了度量加权函数，如下：
其中，
- \(o\)表示特征（e表示边缘，s表示平面）
- 对于边缘线特征，\(\nu_e, n_e\)分别表示在\(p_k^l\)处的边缘线的方向向量，和最近5个边缘点特征所近似得到的边缘线
- 对于平面特征，\(\nu_s,n_s\)分别表示在点\(p_k^l\)处的平面法向量和由最近5个点形成的
- \(\gamma(p_k^l),\gamma_j\)分别代表特征点\(p_k^l\)及其关联的最近5个特征点的关联值
因此，建议的度量加权函数考虑了特征关联的几何和外观一致性
基于关键帧的imu-lidar紧耦合滑动窗口优化
基于关键帧的策略首先应用于视觉slam，LIO-SAM，使用isam2对前端激光前端里程计的关键帧和imu预积分约束进行融合，liom中紧密耦合的LiDAR惯性里程计系统通过滑动窗口优化实现了LiDAR和预集成IMU测量的直接融合，但是实时性不够，因为融合了每一帧lidar扫描
关键帧策略
因此，保持后端直接LiDAR-惯性融合优化方案的稀疏性非常重要，如图5所示，提出的融合方案利用关键帧建立滑动窗口，其中，激光雷达和imu预积分观测使用非线性优化的方法进行融合。
但是关键帧的选取标准会影响前端里程计的准确性，由于imu积分在相邻两关键帧之间存在漂移，因此，提出了两个准则：
- 当前帧的特征与局部特征地图重叠率<60%
- 如果最后一个关键帧的时间差大于指定阈值
在窗口优化后向前滑动时，构造一个局部因子图，对两个最旧的关键帧位姿作为约束，而regular-frame帧位姿则通过imu测量作为初始化值。使用一个小规模的因子图优化以获取与激光雷达采样频率一致的regular-frame位姿
滑动窗口
目标是求解式(1)中的滑动窗口内关键帧状态，方法是通过求解激光特征点和imu预积分约束构成的非线性优化问题，使用一个包含三个分量的最大后验(MAP)目标函数：
其中，
- \(\breve{X}=[\breve{x}_1^T,\cdots,\breve{x}_2^T]^T \in \R^{15\times \tau_w}\)表示滑动窗口中的关键帧状态，\(\tau_w\)表示滑窗大小
- \(\mathcal{R}_{p}(\breve{X})\)表示由于滑动窗口滑动时进行marginalized得到的关于观测的先验项
- \(\mathcal{R}_L(\breve{x}_k^w,\breve{p}_{k,i}^l,\breve{\Mu}^w)\)表示激光雷达残差项（根据待估计的关键帧位姿\(\breve{x}_k^w\)将观测到的点\(\breve{p}_{k,i}^l\)对齐到局部特征点地图\(\breve{\Mu}^w\)），m表示特征关联数
- 局部特征点地图\(\breve{\Mu}^w\)结合了最近30帧关键帧观察到的特征
- \(\mathcal{R}_J(\breve{x}_k,\hat{z}_{k+1}^k)\)是第k个关键帧以及imu数据从第k帧到k+1帧的imu预积分约束所构建的残差项
先验因子
为了在不造成实质性信息损失的情况下限制LiDAR惯性融合的计算负担，我们在基于关键帧的窗口中利用边缘化，vins-mono中，使用Schur-complement对最旧的关键帧及其观测进行边缘化，相应的计算得到一个新的先验，并将其添加到一存在的先验因子的顶部。
激光雷达残差
加权的点-线、点-面距离
imu观测残差（预积分误差）
为了避免每次优化窗口滑动时都重新传播IMU状态，所以将两个连续的关键帧k和k+1之间的imu原始数据进行积分，并将预积分看作是两个关键帧之间的相对位姿观测，可以得到预积分误差：
其中，
- \(\Delta \tau_k\)表示第k关键帧与k+1帧的时间间隔
- \([\cdot]_{2:4}\)表示取四元数向量中的最后3个元素
- \(\hat{z}_{k+1}^{k}=[\hat{\alpha}_{k+1}^{kT},\hat{\beta}_{k+1}^{kT},\hat{\gamma}_{k+1}^{kT}]\)表示从第k帧到第k+1帧的imu预积分量
- 由于篇幅所限，论文不提供IMU预积分的推导和相应的噪声协方差\(C_{k+1}^{k}\),具体可参考vins-mono