xjturmy的博客

patchwork++源码中参数变量较多，并且除了R-VPF（区域垂直平面拟合），R-GPF（区域地面拟合），A-GLE（自适应地面似然估计）和TGR（ 临时地面还原）这些主要功能模块，还有很多处理的细节，需要先对算法的主要逻辑框架进行梳理，然后再对每个功能模块的细节进行分析。

pathwork++中同心区域模型描述比较好理解，但是在代码中哪个变量对应哪个区域的控制量，尤其是代码中变量的定义容易理解错误，因而增加了CZM区域可视化的功能，便于直观的理解算法的边界。

工作中需要用到去地面算法，调研发现pathwork++去地面效果比较好，但是在实际使用中依然无法解决遇到的问题，这里进行源码不同模块的理解，提升自己算法思维和算法开发能力。

针对第一版中算法耗时过长，达到了30ms，需要对各个环节的耗时情况分析，减少不必要的算法逻辑。考虑到每一步实际上只是做了一个判断，采用多线程并不能很好的降低耗时，参考如下多线程不一定提高效率算法优化目前整体耗时在6-7ms左右，基本满足需要测试过程中的代码并没有很好的保存下来，因此每个部分记录的代码比较乱以后再做优化，切个分支单独记录最后做了代码整体格式的优化，和第一版有较大出入，很多变量都不同，这里主要记录思路和效果后续依然后优化空间，主要减少误识别的情况，有时间单独对效果进行优化分析。

在实际测试中，选取园区场景进行测试，降采样后点云数量为6W个左右，基于法向量的去地面算法的算法耗时太长，而基于平面拟合的算法对于同时存在坡度和转弯的的地面处理效果极差，因此选择基于坡度的地面过滤算法实现。基于坡度的过滤地面算法实现。

基于传统的感知算法包括去地面点云，障碍物聚类，目标跟踪和障碍物信息输出等，去地面算法绝对这感知算法中障碍物识别效果，因此对一般算法进行调研。