FAST-LIVO2中的体素地图内存管理优化实践

背景介绍

FAST-LIVO2作为一款先进的激光-视觉惯性里程计系统，在实际应用中面临着大规模场景下的内存管理挑战。特别是在默认参数下未启用地图滑动功能(map_sliding_en)时，系统会随着运行时间的增加而累积大量体素数据，导致内存占用不断攀升。这一问题在资源受限的嵌入式平台(如Orin NX)上尤为突出。

原有方案的局限性

FAST-LIVO2原有的内存管理策略存在两个主要问题：

1. 性能瓶颈：当启用map_sliding_en时，随着地图规模扩大，单次滑窗移除数据的耗时显著增加，实测可达30-40ms甚至更高，严重影响系统实时性。

2. 内存控制不足：基于空间范围的移除机制未能有效控制点云数量，内存占用仍会持续增长。

LRU策略的优化方案

针对上述问题，社区开发者提出了基于LRU(最近最少使用)算法的优化方案。LRU是一种经典的缓存淘汰算法，其核心思想是"如果数据最近被访问过，那么将来被访问的概率也更高"。

实现原理

优化方案采用了双数据结构设计：

• 使用哈希表(unordered_map)快速查找体素
• 使用双向链表(list)维护访问顺序

具体实现包含两个关键组件：

1. voxel_map_cache：存储体素位置和指针的链表，按访问时间排序
2. voxel_map：哈希表，快速定位链表中的体素

实现细节

优化后的关键操作流程：

1. 体素访问：

   • 命中缓存：将对应节点移至链表头部
   • 未命中：创建新体素并插入链表头部

2. 容量控制：

   • 设置最大体素数量阈值(如20000)
   • 当超出阈值时，移除链表尾部的最久未使用体素

性能优势

1. 时间复杂度优化：

   • 原方案：O(N)遍历整个voxel_map
   • LRU方案：O(1)的哈希查找和链表操作

2. 内存控制：

   • 精确控制体素数量上限
   • 避免无效数据残留

3. 实测效果：

   • 最高处理耗时从30-40ms降至8ms
   • 平均耗时约1ms
   • 内存占用显著降低且不会无限增长

工程实现建议

在实际工程实现中，开发者提供了两种优化思路：

1. 即时删除方案：

   // 插入新体素时检查容量
   if (voxel_map_cache_.size() > MAX_VOXEL_NUM) {
       auto old_it = std::prev(voxel_map_cache_.end());
       voxel_map_.erase(old_it->first);
       delete old_it->second;
       voxel_map_cache_.pop_back();
   }
   

2. 批量删除方案：

   // 处理完所有点云后统一清理
   if (voxel_map_cache_.size() >= CAPACITY) {
       while (voxel_map_cache_.size() >= CAPACITY) {
           delete voxel_map_cache_.back().second;
           auto last_key = voxel_map_cache_.back().first;
           voxel_map_.erase(last_key);
           voxel_map_cache_.pop_back();
       }
   }
   

批量删除方案在实际测试中表现出更好的效率，因为减少了遍历过程中的删除操作。

扩展优化

除了LIO部分的体素地图，VIO模块中的特征地图(feat_map)也存在类似的内存增长问题。开发者建议同样采用LRU策略进行优化，但需要注意：

1. VIO模块中存在多个feat_map实例(类成员和函数参数)
2. 需要谨慎处理多地图间的数据一致性
3. 优化后同样能获得显著的内存节省效果

总结

FAST-LIVO2通过引入LRU缓存淘汰机制，有效解决了大规模场景下的内存管理问题。该优化方案具有以下特点：

1. 算法复杂度从O(N)降至O(1)
2. 精确控制内存使用上限
3. 保持原有精度不变
4. 适用于资源受限的嵌入式平台
5. 可扩展至系统其他模块

这一优化实践为类似SLAM系统的内存管理提供了有价值的参考，特别是在需要长时间运行的场景中，能够保证系统的稳定性和实时性。