文章目录阶段二LIO 前端 - 深入学习 IVox 局部地图一、什么是 IVox二、核心成员变量三、核心函数 1AddPoints() - 添加点云四、核心函数 2GetClosestPoint() - 最近邻搜索五、核心函数 3GenerateNearbyGrids() - 预先生成邻居格子六、Options 配置参数七、IVox 的优点总结阶段二LIO 前端 - 总结下一步阶段二LIO 前端 - 深入学习 IVox 局部地图这是 Lightning-LM 里一个很有特色的模块是一种高效的空间数据结构一、什么是 IVox看 ivox3d.hIVox Incremental Voxel 增量式体素地图IVox 的核心功能 添加点云 AddPoints() 最近邻搜索 GetClosestPoint() → 这是 LIO 匹配时要用的先看类的模板声明templateintdim3,IVoxNodeType node_typeIVoxNodeType::DEFAULT,typenamePointTypepcl::PointXYZclassIVox二、核心成员变量Options options_;// 配置参数// voxel 哈希表key → (grid_key, voxel_node) 的迭代器std::unordered_mapKeyType,typenamestd::liststd::pairKeyType,NodeType::iterator,math::hash_vecdimgrids_map_;// voxel 缓存列表用 list 是为了方便 splice 操作std::liststd::pairKeyType,NodeTypegrids_cache_;// 要搜索的“邻居格子”std::vectorKeyTypenearby_grids_;数据结构的巧妙之处 grids_map_ 哈希表用于 O(1) 查找某个网格是否存在grids_cache_ 双向链表用于实现 最近最少使用LRU nearby_grids_ 预先生成的“邻居格子”的坐标偏移三、核心函数 1AddPoints() - 添加点云看 ivox3d.h这是增量式的核心voidIVox::AddPoints(constPointVectorpoints_to_add){std::for_each(points_to_add.begin(),points_to_add.end(),[this](constautopt){// 1. 把点的坐标转成网格坐标autokeyPos2Grid(math::ToEigenfloat,dim(pt));// 2. 查找这个网格是否已经存在autoitergrids_map_.find(key);if(itergrids_map_.end()){// 3a. 不存在创建新网格PointType center;center.getVector3fMap()key.templatecastfloat()*options_.resolution_;// 放到链表的最前面grids_cache_.push_front({key,NodeType(center,options_.resolution_)});grids_map_.insert({key,grids_cache_.begin()});grids_cache_.front().second.InsertPoint(pt);// 4. 如果超过容量删除最久未使用的链表最后一个if(grids_map_.size()options_.capacity_){grids_map_.erase(grids_cache_.back().first);grids_cache_.pop_back();}}else{// 3b. 已存在直接插入点iter-second-second.InsertPoint(pt);// 5. 关键把这个网格移到链表最前面表示刚用过grids_cache_.splice(grids_cache_.begin(),grids_cache_,iter-second);grids_map_[key]grids_cache_.begin();}});}关键点 1LRU 策略用 list 的 splice 操作把刚访问过的网格移到最前面当容量满时删除链表最后面的最久未使用的关键点 2Pos2Grid() 函数KeyTypePos2Grid(constPtTypept)const{return(pt*options_.inv_resolution_).array().round().templatecastint();}把三维坐标转成网格坐标整数比如 resolution 0.2 点 (0.3, 0.3, 0.3) → 网格 (1, 1, 1)四、核心函数 2GetClosestPoint() - 最近邻搜索看 ivox3d.h这是 LIO 匹配时调用的先看单最近邻版本boolIVox::GetClosestPoint(constPointTypept,PointTypeclosest_pt){std::vectorDistPointcandidates;autokeyPos2Grid(math::ToEigenfloat,dim(pt));// 1. 在“邻居格子”里找候选点std::for_each(nearby_grids_.begin(),nearby_grids_.end(),[](constKeyTypedelta){autodkeykeydelta;autoitergrids_map_.find(dkey);if(iter!grids_map_.end()){DistPoint dist_point;boolfounditer-second-second.NNPoint(pt,dist_point);if(found)candidates.emplace_back(dist_point);}});if(candidates.empty())returnfalse;// 2. 在候选点里找最近的autoiterstd::min_element(candidates.begin(),candidates.end());closest_ptiter-Get();returntrue;}再看多最近邻版本boolIVox::GetClosestPoint(constPointTypept,PointVectorclosest_pt,intmax_num,doublemax_range){// ...// 1. 在邻居格子里收集候选点for(constKeyTypedelta:nearby_grids_){autodkeykeydelta;autoitergrids_map_.find(dkey);if(iter!grids_map_.end()){iter-second-second.KNNPointByCondition(candidates,pt,max_num,max_range);}}// 2. 用 nth_element 快速找到前 max_num 个最近的不需要全排序if(candidates.size()max_num){std::nth_element(candidates.begin(),candidates.begin()max_num-1,candidates.end());candidates.resize(max_num);}// ...}关键技巧 std::nth_element不需要把所有点都排序只需要把第 max_num 个位置的元素放到正确位置左边的都比它小右边的都比它大复杂度是 O(n)比排序 O(n log n) 快五、核心函数 3GenerateNearbyGrids() - 预先生成邻居格子看 ivox3d.hvoidIVox::GenerateNearbyGrids(){if(options_.nearby_type_NearbyType::CENTER){// 只搜索中心格子nearby_grids_.emplace_back(KeyType::Zero());}elseif(options_.nearby_type_NearbyType::NEARBY6){// 搜索中心 前后左右上下 6 个邻居nearby_grids_{KeyType(0,0,0),KeyType(-1,0,0),KeyType(1,0,0),KeyType(0,1,0),KeyType(0,-1,0),KeyType(0,0,-1),KeyType(0,0,1)};}elseif(options_.nearby_type_NearbyType::NEARBY18){// 搜索 18 个邻居面相邻 边相邻nearby_grids_{...};}elseif(options_.nearby_type_NearbyType::NEARBY26){// 搜索 26 个邻居面相邻 边相邻 角相邻nearby_grids_{...};}}为什么预先生成避免每次搜索时重复计算空间换时间六、Options 配置参数structOptions{floatresolution_0.2;// 格子大小分辨率floatinv_resolution_10.0;// 1/resolutionNearbyType nearby_type_NearbyType::NEARBY6;// 搜索范围std::size_t capacity_1000000;// 最大格子数};参数选择建议 resolution_ 0.1~0.3越小越准但越慢nearby_type_ 通常 NEARBY6 就够了capacity_ 根据场景大小调整大场景设大一点七、IVox 的优点总结阶段二LIO 前端 - 总结现在 LIO 前端的所有核心模块都学完了点云预处理 统一不同雷达数据抽稀滤波IMU 处理 初始化去畸变ESKF 滤波器 预测更新迭代优化LaserMapping 主流程 把这些模块串起来IVox 局部地图 增量式高效最近邻搜索下一步接下来可以学习 后端 和 定位模块 回环检测 loop_closing.h/cc 位姿图优化 pose_graph/ 定位系统 loc_system.h/cc 地图管理 maps/tiled_map.h/cc