栅格地图、障碍物地图与膨胀地图（膨胀地图（二）写一张膨胀地图）

2024-06-20 00:12
文章标签 地图 栅格 膨胀 一张 障碍物

本文主要是介绍栅格地图、障碍物地图与膨胀地图（膨胀地图（二）写一张膨胀地图），希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

前面看完了膨胀地图相关的内容，这里根据前面看过的内容手搓一张膨胀地图试一下。

1、数据预处理

第一步,先进行数据预处理，为了后续计算方便，首先在这里预先计算两张二维数组表，后续遍历时会用到这张表：

void map_test3::computeCaches()
{//Case 1:如果栅格的膨胀半径是0，那就直接返回。if (robot_radius == 0)return;cached_distances_.clear(); // 分配内存for (unsigned int i = 0; i <= 50; ++i) {for (unsigned int j = 0; j <= 50; ++j) {cached_distances_[i][j] = hypot(static_cast<double>(i), static_cast<double>(j));ROS_INFO("i:%d,j:%d,value:%f",i,j,cached_distances_[i][j]);}}ROS_INFO("computeCost");cached_costs_.clear();// 计算代价for (unsigned int i = 0; i <= 50; ++i) {for (unsigned int j = 0; j <= 50; ++j) {cached_costs_[i][j] = computeCost(hypot(static_cast<double>(i), static_cast<double>(j)));//ROS_INFO("i:%d,j:%d,value:%f",i,j,cached_costs_[i][j]);}}
}

这个函数内涉及到了两张表，第一张存储的是三角形的斜边长，它的意义是当前栅格在离它最近的障碍物点之间的栅格距离。而第二章表存储的是代价值，它与第一张表是相对应的，代表的是这个距离的cost值的变化。这里面还用到了一个computeCost函数，它与源码中的基本上是差不多的，只是源码中的变量weight这里暂时使用了固定值代替，但是不影响大体运行思路。

2、地图订阅与膨胀

在两张表处理完成后，接下来需要一张现有的地图，所以这里要进行一张地图的订阅与数据处理。地图的订阅比较简单，就是将数据存储到raw_data内，但是需要注意一个问题，那就是我们是不能直接对这张地图进行膨胀的，因为如果障碍物紧贴地图边缘的话，根据机器人大小向外膨胀时会超出地图边界，这样的话数据计算上会变得比较麻烦，因此，这里获取到原始数据后首先对其按照机器人大小进行膨胀，这样子后续就不用担心处理边界的问题：

void map_test3::updateBounds()
{int length = int(robot_radius/map_resolution)+1;ROS_INFO("length:%d",length);for(int i=0;i<map_height+2*length;i++){for(int j=0;j<map_width+2*length;j++){if(i<length)  //地图上边界膨胀{expend_data.push_back(-1);}else if(length <= i && i<map_height+length){if(j<length){expend_data.push_back(-1);}else if(length<=j && j<map_width+length){expend_data.push_back(raw_data[map_width*(i-length)+j-length]);}else{expend_data.push_back(-1);}}else{expend_data.push_back(-1);}}}
}

updateBounds函数的中心思想还是很简单的，它只需要将原始数据按顺序保留，在地图外部扩张出一部分栅格即可，对于这些扩张出来的栅格，先可以将其设置为未知（-1）。

3、障碍物点云的提取与膨胀

这一部分的思想与上一篇中源码的思想基本上是一致的，首先提取出地图中所有的障碍物点，然后对其进行遍历，更改其占用值，当然，对于这些点而言，它的占用值是不变的，然后，下一步更新其周围四个点的数据：

void map_test3::updateCosts()
{int length = int(robot_radius/map_resolution)+1;int seen_size_ = (map_height+2*length) * (map_width+2*length);if (seen_ == NULL) {// seen_的阵列为空，重新赋值ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL");int seen_size_ = (map_height+2*length) * (map_width+2*length);seen_ = new bool[seen_size_]();}else if (seen_size_ != (map_height+2*length) * (map_width+2*length)){ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong");// seen_的阵列为错误，删除后重新赋值delete[] seen_;seen_size_ = (map_height+2*length) * (map_width+2*length);seen_ = new bool[seen_size_]();//这里分配了一个size 和master map 尺寸一样的bool 数组}else{if (seen_)delete[] seen_;seen_size_ = (map_height+2*length) * (map_width+2*length);seen_ = new bool[seen_size_]();}inflation_cells_.clear();inflation_cells.clear();map_seen.clear();for (int i = 0; i < map_height+2*length; i++){for (int j = 0; j < map_width+2*length; j++){int index = i*(map_width+2*length)+j;    //1)从master_grid地图获取(i,j)对应的索引indexunsigned char cost = expend_data[index];  //2)从master_grid地图获取索引对应的代价costif (cost == 100)               //3)先判断代价是否等于致命障碍物对应的代价，如果是，压入{ //注意这里的obs_bin是一个指针，数据其实是存储在inflation_cells_下//std::map<double, std::vector<CellData> > inflation_cells_;CellData celldata;celldata.index_ = index;celldata.src_x_ = i;celldata.src_y_ = j;celldata.x_ = i;celldata.y_ = j;inflation_cells_[0].push_back(celldata);inflation_cells.push_back(celldata);}}}//根据膨胀队列，进行膨胀//通过上面的操作，现在inflation_queue_里面全部都是obstacle cell，这些cell被打包成CellData类型，包含了这些cell本身的索引，最近障碍物的索引（即它们本身），距离（0）std::map<double, std::vector<CellData> >::iterator bin;int count = 0;int interate = 0;while(inflation_cells.size()>0)// && count<12000){CellData cell;cell = inflation_cells[0];//记录该celldata的索引indexunsigned int index = cell.index_;// ignore if already visited  如果已访问过，则忽略if (seen_[index]){//ROS_INFO("index: %d has been seen",index);inflation_cells.erase(inflation_cells.begin());continue;}seen_[index] = true;map_seen[index] = true;//得到该cell的坐标和离它最近的障碍物的坐标unsigned int mx = cell.x_;unsigned int my = cell.y_;unsigned int sx = cell.src_x_;unsigned int sy = cell.src_y_;//ROS_INFO("mx:%d,my:%d,src_x:%d,src_y:%d",mx,my,sx,sy);//4)更新膨胀队列中点的cost 值//根据该CELL与障碍的距离来分配costunsigned char cost = costLookup(mx, my, sx, sy);unsigned char old_cost = expend_data[index];  //从master_grid查找指定索引index的代价//如果old_cost无信息或者cost≥内接膨胀障碍物，将cost更新到master_arrayif (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))expend_data[index] = cost;else//否则比较新旧大小，将大的cost更新到master_arrayexpend_data[index] = std::max(old_cost, cost);ROS_INFO("index:%d",index);// attempt to put the neighbors of the current cell onto the inflation list//上面首先从inflation_cells_中取出最大distance的cell，再将这个cell的前后左右四个cell都塞进inflation_cells_。//由于下面关于enqueue的调用，最后两个参数（sx, sy)没有改变，所以保证了这个索引一定是obstacle cell。//由于在 enqueue入口会检查 if (!seen_[index])，这保证了这些cell不会被重复的塞进去。//由于这是一个priority queue，因此障碍物的周边点，每次都是离障碍物最远的点被弹出，并被检查，这样保证了这种扩张是朝着离障碍物远的方向进行。//尝试将当前单元的邻居(前后左右)放到队列中 //ROS_INFO("COUNT:%d",count);//if (mx > 0 && count<1000)if (mx > 0)enqueue(index - 1, mx - 1, my, sx, sy);if (my > 0)enqueue(index - (map_width+2*length), mx, my - 1, sx, sy);       if (mx < (map_width+2*length) - 1)enqueue(index + 1, mx + 1, my, sx, sy);if (my < (map_height+2*length) - 1)enqueue(index + (map_width+2*length), mx, my + 1, sx, sy);inflation_cells.erase(inflation_cells.begin());    }
}

这个函数的中心在于while循环以及循环内的enqueue函数，enqueue函数是非常重要的一个函数，它的主要内容如下：

inline void map_test3::enqueue(int index, unsigned int mx, unsigned int my,unsigned int src_x, unsigned int src_y)
{if (!seen_[index]){// we compute our distance table one cell further than the inflation radius dictates so we can make the check below//找它和最近的障碍物的距离，如果超过了阈值，则直接返回//如果不超过阈值，就把它也加入inflation_cells_数组里double distance = distanceLookup(mx, my, src_x, src_y);// we only want to put the cell in the list if it is within the inflation radius of the obstacle pointif (distance*map_resolution > robot_radius)return;// push the cell data onto the inflation list and markCellData celldata;celldata.index_ = index;celldata.x_ = mx;celldata.y_ = my;celldata.src_x_ = src_x;celldata.src_y_ = src_y;inflation_cells_[distance].push_back(celldata);inflation_cells.push_back(celldata);}
}

在enqueue函数中会判断当前点到最近障碍物的距离。如果处于一个危险范围内(distance*map_resolution < robot_radius)则会将该点加入到遍历的队列，后续会根据点到障碍物点的距离更新其代价值，如果这个点离障碍物足够远则可以认为当前这个点是足够安全的，则不需要更新该点，通过这种方式，可以将所有离障碍物点足够近的点都遍历一遍，并修改这些点的占用值为合适的数据。

地图的重新发布

在经过第三步的遍历后，也就修改完成了所有栅格点的占用数值，然后，我们将其重新发布出来，得到一张处理后的地图，类似于如下状态：
在这里插入图片描述
后续就可以在这张地图上进行进一步的处理，例如结合障碍物地图以及路径规划实现半动态的路径规划：
在这里插入图片描述

小结

通过地图的遍历以及每个点到障碍物的距离与代价值一一对应，可以在一次遍历下完成地图的膨胀，整体来说算法的效率还是比较高的。通过该部分算法再结合激光点云以及其他算法，可以实现很多需要的功能，例如全局路径规划等。

这篇关于栅格地图、障碍物地图与膨胀地图（膨胀地图（二）写一张膨胀地图）的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/1076555。

相关文章

无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

墙体画线地图切分方案 针对问题：墙体两侧特征混淆误匹配，导致建图和定位偏差，表现为过门跳变、外月台走歪等 ·解决思路：预期的根治方案IGICP需要较长时间完成上线，先使用切分地图的工程化方案，即墙体两侧切分为不同地图，在某一侧只使用该侧地图进行定位 方案思路 切分原理：切分地图基于关键帧位置，而非点云。 理论基础：光照是直线的，一帧点云必定只能照射到墙的一侧，无法同时照到两侧实践考虑：关
阅读更多...
MiniGPT-3D, 首个高效的3D点云大语言模型，仅需一张RTX3090显卡，训练一天时间，已开源

MiniGPT-3D, 首个高效的3D点云大语言模型，仅需一张RTX3090显卡，训练一天时间，已开源

项目主页：https://tangyuan96.github.io/minigpt_3d_project_page/ 代码：https://github.com/TangYuan96/MiniGPT-3D 论文：https://arxiv.org/pdf/2405.01413 MiniGPT-3D在多个任务上取得了SoTA，被ACM MM2024接收，只拥有47.8M的可训练参数，在一张RTX
阅读更多...
全英文地图/天地图和谷歌瓦片地图杂交/设备分布和轨迹回放/无需翻墙离线使用

全英文地图/天地图和谷歌瓦片地图杂交/设备分布和轨迹回放/无需翻墙离线使用

一、前言说明 随着风云局势的剧烈变化，对我们搞软件开发的人员来说，影响也是越发明显，比如之前对美对欧的软件居多，现在慢慢的变成了对大鹅和中东以及非洲的居多，这两年明显问有没有俄语或者阿拉伯语的输入法的增多，这要是放在2019年以前，一年也遇不到一个人问这种需求场景的。 地图应用这块也是，之前的应用主要在国内，现在慢慢的多了一些外国的应用场景，这就遇到一个大问题，我们平时主要开发用的都是国内的地
阅读更多...
c++/《一张图看懂C++编程规范》

c++/《一张图看懂C++编程规范》

阅读更多...
Imageview在百度地图中实现点击事件

Imageview在百度地图中实现点击事件

1.首先第一步，需要声明的全局有关类的引用 private BMapManager mBMapMan; private MapView mMapView; private MapController mMapController; private RadioGroup radiogroup; private RadioButton normalview; private RadioBu
阅读更多...
MMO地图传送

MMO地图传送

本篇由以下四个点讲解： 创建传送点 传送点配置 编辑器扩展：传送点数据生成 传送协议与实现 创建传送点 建碰撞器触发 //位置归零 建一个传送门cube放到要传送的位置（这个teleporter1是传出的区域 这是从另一张地图传入时的传送门 创建一个脚本TeleporterObject给每个传送cube都绑上脚本 通过脚本，让传送门在编辑器下面还能绘制出来
阅读更多...
Oracle把一个表的某个字段更新到另一张表中

Oracle把一个表的某个字段更新到另一张表中

第一种方法： update tablea set column_name1=(select name2 from tableb where tableb.name3=tablea.name1) 只修改一个 update tablea set column_name1=(select name2 from tableb where tableb.name3='a') where tablea.na
阅读更多...
1-9 图像膨胀 opencv树莓派4B 入门系列笔记

1-9 图像膨胀 opencv树莓派4B 入门系列笔记

目录 一、提前准备 二、代码详解 kernel = np.ones((3, 3), np.uint8) _, binary_image = cv2.threshold(image, 127, 255, cv2.THRESH_BINARY) dilated_image = cv2.dilate(binary_image, kernel, iterations=1) 三、运行现象 四
阅读更多...
中国生态环境胁迫数据（栅格/县域尺度）-为研究生态环境压力提供数据支撑

中国生态环境胁迫数据（栅格/县域尺度）-为研究生态环境压力提供数据支撑

中国生态环境胁迫矢量数据（2000-2010年） 数据介绍 2000-2010年中国生态环境胁迫数据为2000-2010年中国范围内人口、农业生产等生态环境胁迫因子的空间分布图，包括人口密度、农药使用强度、化肥施用强度。数据可用于分析全国生态环境胁迫因子及其对生态环境造成的压力的空间特征，主要通过社会经济统计资料获得，为县域尺度空间数据。 存储容量31.01 GB文件数量6数据类型栅
阅读更多...
ArcGIS Pro SDK （十三）地图创作 3 特殊图层

ArcGIS Pro SDK （十三）地图创作 3 特殊图层

ArcGIS Pro SDK （十三）地图创作 3 特殊图层 文章目录 ArcGIS Pro SDK （十三）地图创作 3 特殊图层1 高程表面图层1.1 创建具有地表图层的场景1.2 创建新的高程表面1.3 将自定义高程表面设置为 Z 感知图层1.4 将高程源添加到现有高程表面图层1.5 从地图中获取高程表面图层和高程源图层1.6 查找高程表面图层1.7 移除高程表面图层1.8 从曲面获
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2