fuel无人机自主探索代码解读2——sdf_map.cpp【占据栅格地图、膨胀占据栅格地图、ESDF】

一、概述

sdf_map.cpp负责map_ros.cpp内部主体函数的具体实现，主要功能包括：

1. 融合输入的深度或点云图生成全局占据栅格地图
2. 根据launch参数生成全局膨胀占据栅格地图
3. 基于膨胀占据栅格地图采用欧式距离传输生成局部的ESDF地图
4. 采用三线性插值的方式得到某点的esdf值及梯度

备注：map_ros.cpp的主要函数说明：fuel无人机自主探索代码解读1——map_ros.cpp【地图ros接口】

二、主要函数及作用

1、SDFMap::initMap

作用：sdf map初始化
1、地图launch参数读取及相关数据生成
2、膨胀地图参数读取及相关数据生成
3、珊格地图、膨胀珊格地图、esdf等向量初始化
4、探索相关数据赋值
5、地图ros接口类初始化
6、ray-cast参数设置

2、SDFMap::resetBuffer()

作用：缓存重置
1、地图下边界珊格-----》地图上边界珊格：将对应珊格的膨胀占据珊格地图置于0；esdf参数值变更为初始值0.0
2、对当前帧点云AABB包围框的下界变为【0，0，0】索引，上界变为各轴方向的珊格总个数

3、SDFMap::resetBuffer(const Eigen::Vector3d& min_pos, const Eigen::Vector3d& max_pos)

作用：地图下边界珊格-----》地图上边界珊格范围内：将对应珊格的膨胀占据珊格地图置于0；esdf参数值变更为初始值0.0

4、SDFMap::fillESDF

输入【以z轴进行esdf更新为例（esdf先z后y再x）】：
1、匿名函数1：输入当前珊格z值，若膨胀珊格被占据则返回0，否则返回无穷大     
2、匿名函数2：输入珊格z索引及对应的esdf值，作用对对应的珊格位置的buffer进行esdf赋值      3、最小z值    
4、最大z值    
5、2【地图某一维珊格数量】
作用：对某一维度的珊格进行esdf值的求解。

5、SDFMap::updateESDF3d()

作用：针对膨胀后的局部点云帧包围框采用欧式距离传输方法进行esdf的计算【最近障碍物距离，结果存储在distance_buffer_中】
注意：无符号esdf：占据或未知为0，空白为正     有符号esdf：障碍物边缘珊格值为0，内部珊格值为负数，空白珊格为正

6、SDFMap::setCacheOccupancy

输入：
1、某雷达近似点对应的栅格索引的一位数组坐标    
2、是否占据标志位置【0表示空、1表示占据】
作用：给定的一维数组坐标压入缓存体素【当前帧点云该位置被首次访问】；若栅格空闲则count_miss_对应位置置于1；若栅格占据则count_hit_对应位置+1

7、SDFMap::inputPointCloud

输入：
1、世界坐标系下深度点云     
2、点云个数      
3、world坐标系下相机位置
作用：
1、寻找点云点的近似点【考虑考虑地图最大边界限制、相机的最大照射范围】，将相机位置到近似点做连线ray-cast，确定该连线上栅格位置occupancy_buffer_值
2、确定当前帧点云AABB包围框的上下边界【xy膨胀，z不变】后的局部包围框尺寸【钳制与地图范围内】
3、当前帧激光雷达点云修正后的最小和最大范围【考虑当前帧点云位置、相机位置、上一次边界搜索时相机位置【时间间隔不长】】

8、SDFMap::closetPointInMap

输入：
1、某激光点云点【超出地图范围】     
2、传感器位姿
作用：以传感器为中心，给定激光雷达点连线与地图边界的交点【返回值】

9、SDFMap::clearAndInflateLocalMap()

作用：
1、遍历膨胀后的当前帧点云AABB包围框内珊格：对于占据珊格，沿着前后上下左右6个方向膨胀给定珊格个数，对膨胀珊格集合进行occupancy_buffer_inflate_置1操作，其余为0
2、如果天花板高度大于-0.5，将对当前帧点云AABB包围框的上下边界【xy膨胀，z不变】后的局部包围框的天花板位置的珊格的occupancy_buffer_状态置于2.197

10、SDFMap::getResolution()

作用：得到sdf地图的栅格分辨率

11、SDFMap::getVoxelNum()

作用：得到整个地图范围的栅格总数

12、SDFMap::getRegion

作用：
1、得到地图原点坐标【地图的中心点坐标为【0，0】，左下角为地图原点，高度为给定的地面高度-1.0】
2、地图xyz三轴方向的尺寸

13、SDFMap::getBox

作用：
获取被探索区域的边界框【launch读取】（double）：下界限     上界限

14、SDFMap::getUpdatedBox

输入：
1、3d向量【点云范围的最小坐标】     
2、3d向量【点云范围的最大坐标】     
3、true
作用：
得到当前点云的最小坐标【3d】和最大坐标【3d】

15、SDFMap::getDistWithGrad

输入：
1、给定真实位置     
2、给定位置处的梯度值【欲求】
返回值：给定位置的esdf值【经过三线性插值处理】
作用：采用三线性插值的方式得到给定位置的esdf数值及对应梯度值

三、参考资料

1、ray-cast
2、esdf更新
3、三线性插值