livox_mapping的地面约束
时间: 2023-10-06 19:13:53 浏览: 84
livox_mapping 是一种基于 Livox 雷达的激光 SLAM 实现,其中也包含了地面约束。地面约束的作用是约束机器人所在平面与地面平行,避免因为地形变化等原因导致的误差累积。
livox_mapping 中的地面约束可以分为两个部分:
1. 地面点云提取
livox_mapping 使用了基于局部窗口扫描的方法提取地面点云。具体来说,对于每一个激光点,livox_mapping 会选取其周围一定范围内的激光点进行统计,并计算这些激光点的平均高度。如果该点的高度与平均高度之差小于一定阈值,则认为该点属于地面点云。经过这样的处理后,livox_mapping 得到了一份地面点云数据。
2. 地面约束因子
livox_mapping 中对地面约束的建模采用了与前面介绍的因子图优化类似的方法。具体来说,livox_mapping 将机器人的位姿拆分成平移和旋转两个部分,然后对平移部分施加地面约束。地面约束的公式与前面介绍的相同,即:
$$
p_{z,i} - \frac{a}{c}p_{x,i} - \frac{b}{c}p_{y,i} - \frac{d}{c} = 0
$$
其中,$p_{x,i}$、$p_{y,i}$、$p_{z,i}$ 是机器人位姿下的激光点云坐标,$(a, b, c)$ 是地面的法向量,$d$ 是地面的截距。livox_mapping 中通过计算地面点云的平均法向量来得到地面的法向量和截距。然后,对于每一个机器人位姿,livox_mapping 都会将其对应的激光点云中的点与地面约束因子进行匹配,并将匹配得到的因子加入因子图中进行优化。
总的来说,livox_mapping 的地面约束主要包含了两个部分:地面点云提取和地面约束因子。通过这两个部分的处理,livox_mapping 可以在激光 SLAM 中有效地应对地形变化等因素带来的误差。