【LOAM系列】五：Livox-Loam论文笔记

Livox-Loam

ICRA 2020 香港大学火星实验室 HKU-mars 林家荣

1. 论文

Loam livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV

与LOAM的区别：
1）添加了4种点云剔除策略，
2）增加反射率边缘点，
3）边线和平面匹配时不是在scan扫描线上取点，而是先找最近邻5个点，然后PCA分析判断是否为线或面

1.1 系统流程

**固态激光雷达：**FOV小，规则的扫描方式，非重复扫描，运动模糊(畸变)非重复扫描。
非重复扫描导致提取的特征不能通过扫描线顺序在两帧之间不断匹配。即使激光雷达是静止的，扫描的轨迹（和特征点）也与前一帧不同。

每个新帧直接与全局地图匹配以提供里程计输出。匹配结果反过来用于将帧注册到全局地图，里程计输出和地图更新的速率相同（即 20 Hz）。只有特征点（即边缘点和平面点）保存在内存中，所有原始点都保存在硬盘中以供可能的离线处理

1.2 点云滤波与特征提取

点云深度
$$\begin{array}{c}D(\mathbf{p})=\sqrt{x^2+y^2+z^2}\end{array}$$
点云偏转角
$$\begin{array}{c}\Phi (\mathbf{p})={\tan }^{-1}\left(\sqrt{\left(y^2+z^2\right)/x^2}\right)\end{array}\text{\hspace{1em}(2)}$$
点云强度
$$I(p)=R/D(p{)}^2$$
其中R为点云点的反射率，由激光雷达直接测得，当强度较小时，代表点云点距离较远或者所属物体反射率较低。

入射角 ，是激光线与点云点周围平面的夹角
$$\begin{array}{c}\theta \left({\mathbf{p}}_b\right)={\cos }^{-1}\left(\frac{\left({\mathbf{p}}_a-{\mathbf{p}}_c\right)\cdot {\mathbf{p}}_b}{\left|{\mathbf{p}}_a-{\mathbf{p}}_c\right|\left|{\mathbf{p}}_b\right|}\right)\end{array}\text{\hspace{1em}(3)}$$
筛选掉的点：
1） 接近FoV边缘的点，偏转角大于17°的点，
2）强度过大或过小的点 I(p) ≤ 7 × 10−3,或 I(p) ≥ 1 × 10−1
3）入射角接近π或0的点，角度 <5 或>175度
4）被遮挡的点，判断遮挡，选择扫描顺序上连续的两个点
$$\begin{array}{c}\left|{\mathbf{p}}_e-{\mathbf{p}}_d\right|\ge 0.1\left|{\mathbf{p}}_e\right|,\text{and}\left|{\mathbf{p}}_e\right|>\left|{\mathbf{p}}_d\right|\end{array}$$
特征提取

局部平滑度来提取平面或边线特征，激光雷达的反射率作为四维测量：如果一个点的反射率与附近点的反射率有很大不同，我们也将它视作边线点。

1.3 位姿迭代优化

用ℰk和ℰm分别代表当前帧和当前地图的所有边线特征，对于每一个 ℰk 中的点，寻找它在ℰm中的5个最邻近点。
构建了一个ℰm的KD树,一旦接收到新注册的帧/子帧，就会由另一个并行线程构建KD-tree
对于每一个匹配到的帧中的点投影到全局坐标中
$$\begin{array}{c}{\mathbf{p}}_w={\mathbf{R}}_k{\mathbf{p}}_l+{\mathbf{t}}_k\end{array}\text{\hspace{1em}(4)}$$
其中 (Rk,tk)是当前帧最后一个点采样时LiDAR的位姿, 使用最后一个扫描点时LiDAR的姿态，代表整个当前帧的LiDAR姿态，并且当前帧所有的点云点都以这个姿态来投影到全局地图上

点到边的残差

计算由pw的5 个*最近点形成的均值µ和协方差矩阵Σ，如果Σ的最大特征值是第二大特征值的三倍，我们认为pw的最近点形成一条线，然后用第一个和最后一个点来计算点到线的距离
$$\begin{array}{c}{\mathbf{r}}_{p2e}\left({\mathbf{p}}_w\right)=\frac{\left|\left({\mathbf{p}}_w-{\mathbf{p}}_5\right)\times \left({\mathbf{p}}_w-{\mathbf{p}}_1\right)\right|}{\left|{\mathbf{p}}_5-{\mathbf{p}}_1\right|}\end{array}\text{\hspace{1em}(5)}$$
点到面的残差

对于k帧中的平面点，投影到全局地图后找到地图中5个最近邻点Pm，计算它们的协方差矩阵 Σ 来确保这 5 个最近的点确实在同一平面内。如果Σ的最小特征值比第二小的特征值小三倍，则认为这5个点在统一平面上
$$\begin{array}{c}{\mathbf{r}}_{p2p}\left({\mathbf{p}}_w\right)=\frac{{\left({\mathbf{p}}_w-{\mathbf{p}}_1\right)}^T\left(\left({\mathbf{p}}_3-{\mathbf{p}}_5\right)\times \left({\mathbf{p}}_3-{\mathbf{p}}_1\right)\right)}{\left|\left({\mathbf{p}}_3-{\mathbf{p}}_5\right)\times \left({\mathbf{p}}_3-{\mathbf{p}}_1\right)\right|}\end{array}\text{\hspace{1em}(6)}$$

1.4 帧内运动补偿

1）分段处理

将传入帧分为三个连续的子帧，然后将这三个子帧独立地匹配到全局地图。在每个子帧的扫描匹配过程中，其所有点都使用该子帧终点处的 LiDAR 位姿投影到全局地图。这样，每个子帧的时间间隔为原帧的1/3。多线程计算

2）线性插值

( Rk, tk) 表示当前帧最后一点的 LiDAR 位姿，( Rk −1 , tk−1 ) 表示前一帧最后一个点位姿，(R^k_k - 1,t^k_k - 1)前一帧和当前帧之间的相对旋转和平移，则有t时刻的线性插值位姿为：
$$\begin{array}{c}{\mathbf{R}}_k={\mathbf{R}}_{k-1}{\mathbf{R}}_{k-1}^k,{\mathbf{t}}_k={\mathbf{R}}_{k-1}{\mathbf{t}}_{k-1}^k+{\mathbf{t}}_{k-1}\end{array}$$

$$s=(t-t_{k-1})/(t_k-t_{k-1})$$

$$\begin{array}{c}{\mathbf{R}}_{k-1}^t=e^{\hat{\omega }\theta s},{\mathbf{t}}_{k-1}^t=s{\mathbf{t}}_{k-1}^k\end{array}$$

$$\begin{array}{c}{\mathbf{R}}_{k-1}^t=\mathbf{I}+\hat{\omega }\sin (s\theta )+{\hat{\omega }}^2(1-\cos (s\theta ))\end{array}$$

$$\begin{array}{c}{\mathbf{R}}_t={\mathbf{R}}_{k-1}{\mathbf{R}}_{k-1}^t,{\mathbf{t}}_t={\mathbf{R}}_{k-1}{t}_{k-1}^t+{\mathbf{t}}_{k-1}\end{array}$$

$$\begin{array}{c}{\mathbf{p}}_w(t)={\mathbf{R}}_t{\mathbf{p}}_l+{\mathbf{t}}_t\end{array}$$

都能消除运动模糊，但是线性插值存在长期的漂移，论文认为分段处理优于线性插值

备注：

https://blog.csdn.net/Du_Shuang/article/details/109534783

去畸变步骤

第一步：将当前帧特征点云用上一帧的位姿估计去畸变（补偿到当前帧点云第一个点时刻的位姿），这里只是为了在搜索匹配上一帧的特征点时更加准确，因此没有必要特别准确。
第二步：去畸变之前的当前帧特征点云与前一帧特征点云做帧间匹配，这里在做非线性优化计算残差时进行去畸变处理，即采用迭代的位姿估计做去畸变处理并计算误差，所以最后得到的结果就是当前帧点云采用当前帧位姿估计去畸变，使得两帧之间的投影误差最小。
第三步：将当前帧特征点云去畸变（补偿到当前帧点云最后一个点时刻的位姿），用来给下一帧做匹配。

1.5 外点剔除，动态目标剔除

外点剔除 在迭代姿态优化的每次迭代中，重新找到每个特征点的最近邻点并添加边到边残差和平面到平面残差到目标函数。首先执行少数几次的迭代位姿优化。利用优化的结果，计算两种残差，并去除前20%最大的残差值，去掉以后，再进行完整的迭代位姿优化，

---