广东的网站备案,高级服装定制平台,做网站选什么专业,深圳最大的公司排名一、概述
sdf_map.cpp负责map_ros.cpp内部主体函数的具体实现#xff0c;主要功能包括#xff1a;
融合输入的深度或点云图生成全局占据栅格地图根据launch参数生成全局膨胀占据栅格地图基于膨胀占据栅格地图采用欧式距离传输生成局部的ESDF地图采用三线性插值的方式得到某…一、概述
sdf_map.cpp负责map_ros.cpp内部主体函数的具体实现主要功能包括
融合输入的深度或点云图生成全局占据栅格地图根据launch参数生成全局膨胀占据栅格地图基于膨胀占据栅格地图采用欧式距离传输生成局部的ESDF地图采用三线性插值的方式得到某点的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、地图下边界珊格-----》地图上边界珊格将对应珊格的膨胀占据珊格地图置于0esdf参数值变更为初始值0.0
2、对当前帧点云AABB包围框的下界变为【000】索引上界变为各轴方向的珊格总个数3、SDFMap::resetBuffer(const Eigen::Vector3d min_pos, const Eigen::Vector3d max_pos)
作用地图下边界珊格-----》地图上边界珊格范围内将对应珊格的膨胀占据珊格地图置于0esdf参数值变更为初始值0.04、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_对应位置17、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.19710、SDFMap::getResolution()
作用得到sdf地图的栅格分辨率11、SDFMap::getVoxelNum()
作用得到整个地图范围的栅格总数12、SDFMap::getRegion
作用
1、得到地图原点坐标【地图的中心点坐标为【00】左下角为地图原点高度为给定的地面高度-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、三线性插值
