论文阅读：Visual-Inertial Localization With Prior LiDAR Map Constraints

介绍

提出了一个低代价双目视觉惯导定位系统，实现了基于多状态约束下的卡尔曼滤波器（MSCKF）VIO，采用了先验雷达地图。除了稀疏的视觉特征，雷达地图与半稠密的点云也通过紧耦合的MSCKF进行更新，进而可以纠正漂移。点云和视觉之间的跨模态限制对VIO系统有改善作用。

总之就是提出了VIO状态估计器，其中点云地图提供了先验，考虑到计算效率，采用MSCKF，实现在线定位，仅仅维护一个固定大小的窗口，优化IMU位姿，窗口中不保留特征。同时生成半稠密地图，产生视觉点云，该点云能在雷达地图中进行配准，结果作为相机位姿的全局测量，和视觉稀疏特征以及惯导测量进行紧融合。雷达地图先验通过和视觉半稠密点云以及NDT方法构建。纠正仅VIO偏差，提供6自由度的实时位姿估计。

视觉惯导状态估计

A.状态向量

估计的状态量见公式（1），主要包括全局参考系到第$k$时刻的IMU参考系之间的旋转量、陀螺仪和加速度计偏差、IMU在全局参考系下的位置（这个是不是可以代替平移）、速度、雷达地图和全局参考系之间的位姿变换（旋转+平移），以及一个大小为m帧图像的滑动窗口，维护的状态量是这k帧对应的IMU相对于世界的位姿变换。见公式（2）。状态偏差的定义见公式（3）。状态估计值通过估计值和偏差进行更新，见公式（4）。对于位置和速度更新时直接用加法，位姿量则需要使用四元数乘法。

B.状态传递

状态量和协方差的传递需要对IMU的线加速度和角速度使用IMU运动学进行预积分，见公式（6），参考了文献[30]，EKF传递参考了[1]。

C.状态更新

通过将半密集点云与先验激光雷达地图进行配准，将得到一个具有协方差的配准姿态：

雷达地图约束：考虑时间$k$时左边相机在地图中的位姿，以该测量作为状态函数，可以通过map---global---IMU---相机求解。状态函数线性化以及雅克比矩阵的求解见式（9）-（16）。直接可以使用文献[31]中的EKF进行更新。
视觉特征测量：追踪一些稀疏的特征点，采用滑动窗口。如果特征丢失或者追踪时间过长，则三角化到地图点，并且采用BA优化。成功优化的特征点将通过马氏距离测试，并使用MSCKF更新。稀疏特征点提供短期定位，先验地图可以减小长期定位的漂移。

视觉处理

视觉处理过程主要包括两个方面：稀疏特征点的追踪以及与半稠密视觉点云与雷达地图的配准。这两个过程独立进行，双目构建半稠密点云，和先验雷达地图进行配准，并给出地图和camera之间的变换。

半稠密重建

作者发现对于雷达地图来说，使用视觉重建的点云与其进行陪准时，结构化的表面如平面的配准效果较好，而稀疏重建主要由边和角落对应的高梯度的点组成，因此作者使用半稠密重建，进行配准。论文中选取一些关键帧进行重建，关键帧的选取主要看距离以及姿态变化大小，保证关键帧能覆盖较大的空间区域，且帧之间的重合区域较小，半稠密重建可以参考最近的神经网络方法，见文献[33]。使用窗口维护关键帧（增加约束，便于匹配），对于关键帧先使用双目块匹配计算深度图，即最小化SAD距离，与其他方法相比[34],[35]，传统双目块匹配能保证计算效率，后面还要用其他关键帧进行深度优化。

深度对应匹配

在计算窗口中的关键帧已经通过MSCKF估计出位姿，将关键帧的深度图投影到其他的帧上，计算相似性。深度匹配过程如图4所示，将其他帧的点投到第k帧上，如果像素能匹配上，将点及其深度添加到观测集合上，对于每个要和$j$帧匹配的像素，都要做一个并存测试：

1）两个像素值小于某个阈值

2）两个像素处的梯度小于某个阈值

3）投影过来的深度值相差不能超过阈值

投影时像素位置需要离散化，为了减小误差，投影的是被投影点周围的2*2的块，如果像素块通过了测试，则将投影点以及深度加入集合$P_j(u_n)$上。对于窗口中的每个关键帧都进行该操作，使用观测到的信息对每一帧的点云进行优化，去除外点。如果某个像素的观测点太少，则被认为是外点，如果存在很多观测点，则深度取这些观测点的平均值，该方法使得点云的质量很高，噪声较少。

点云 Assembly

在对每一帧的深度估计进行约束后，需要将每一帧点云投影到当前帧上（参考帧），由于半稠密点云数量太多，作者采用了第二个线程，提供了先验雷达地图约束。只要三个关键帧的窗口，超过半米以及30度的位姿变化就可以很好地约束点云。

NDT点云配准

在参考帧中重建了半稠密点云后，和雷达地图进行配准。NDT方法既保证了精确度也有足够的效率。NDT使用高斯分布来模拟点云，作者采用了P2D方法，实现了源点云到目标点云的配准，求出相对位姿，优化函数见公式（22）。Hessian矩阵的求解见M. Magnusson 的博士论文61页。注意PCL库中的P2D函数使用欧拉角表示姿态，作者使用了四元数参数，需要一个协方差矩阵进行误差状态传递时的转换。

为了降低噪声的影响，文中还采用了以下策略：

1）在计算NDT的Hessian矩阵后，计算其最小特征值，保证其大于一定阈值

2）公式（22）的优化函数代价值小于一定阈值

3）NDT集合中内点与源点云内点的比例反映了配准的质量，应该接近于1（这里源点云是不是应该是重建生成的点云）

4）最终的雷达地图先验约束会进行马氏距离测试处理。

未来工作

考虑雷达地图不确定性，将视觉半稠密地图用于更新雷达先验地图。

A

Assembly 怎么翻译

马氏距离测试是什么？基于马氏距离的异常值检测？

点云地图变化不大，视觉特征地图变化大（光照等）

公式（2）上面which do not evolve over time and are used during feature update:

点云也更新么？深度需要通过匹配优化