FAST_LIO项目中盲区过滤参数失效问题分析与修复

问题背景

在FAST_LIO激光雷达惯性里程计项目中，用户使用MID360激光雷达时发现配置文件中设置的盲区过滤参数(blind)未能生效。该参数本应过滤掉距离雷达过近的点云数据，但在实际运行中并未起到预期效果。

技术分析

通过深入分析FAST_LIO源码，发现问题出在预处理模块(preprocess.cpp)中的条件判断逻辑上。当lidar_type设置为1时，系统会调用avia_handler回调函数处理点云数据。该函数中负责盲区过滤的关键代码段存在逻辑运算符优先级问题。

原始代码中的条件判断语句为：

if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7)
    || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7)
    || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)
    && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z > (blind * blind)))

这段代码存在两个主要问题：

1. 运算符优先级错误：在C++中，逻辑与(&&)的优先级高于逻辑或(||)，导致实际执行顺序与预期不符。

2. 盲区条件被弱化：由于优先级问题，盲区判断条件(距离平方大于blind平方)只有在前面所有坐标差判断都为假时才会被执行，大大降低了盲区过滤的有效性。

解决方案

正确的实现方式是将前三个坐标差判断用括号明确分组，确保它们先作为一个整体进行或运算，然后再与盲区条件进行与运算：

if (((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7)
    || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7)
    || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7))
    && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z > (blind * blind)))

技术原理详解

1. 盲区过滤的意义：激光雷达在近距离测量时会产生噪声点，盲区过滤可以去除这些不可靠的数据，提高建图和定位精度。

2. 距离计算优化：代码中使用距离平方(pl_full[i].x * pl_full[i].x + ...)与blind平方比较，避免了耗时的平方根运算，是常见的性能优化手段。

3. 坐标差判断的作用：1e-7的阈值用于检测连续点是否在同一位置，可能是雷达的无效点或重复点。

影响与验证

该修复对系统带来以下改进：

1. 确保盲区参数按预期工作，有效过滤近距离噪声点
2. 保持原有的有效点云过滤功能
3. 不增加额外的计算负担

用户可以通过以下方式验证修复效果：

1. 设置不同的blind值观察点云数量变化
2. 近距离放置障碍物，检查是否被正确过滤
3. 对比修复前后的建图质量

总结

FAST_LIO项目中的这个盲区过滤问题展示了在复杂条件判断中明确运算符优先级的重要性。通过添加适当的括号明确运算顺序，我们确保了系统参数能够按设计意图正常工作。这类问题也提醒开发者在编写复杂条件判断时要注意运算符优先级，必要时使用括号明确意图，既能避免错误，也提高代码可读性。