ROS区间分割算法实现逻辑分析

最新推荐文章于 2024-05-26 17:09:10 发布

Dakeslon

最新推荐文章于 2024-05-26 17:09:10 发布

阅读量4.4k

点赞数 9

分类专栏：路径规划全覆盖算法 C/C++ 文章标签： c++ 算法人工智能

本文链接：https://blog.csdn.net/DAKESLON/article/details/114106114

版权

C/C++ 同时被 3 个专栏收录

5 篇文章

订阅专栏

路径规划

4 篇文章

订阅专栏

全覆盖算法

4 篇文章

订阅专栏

在上一篇文章中，有一个关键的步骤，是对输入的地图进行区间分割，区间分割的结果直接影响到后续的区间顺序计算以及直线覆盖路径的生成，故本篇对这部分的代码进行分析。

源码文件位置为：autopnp/ipa_room_exploration/common/include/ipa_room_exploration/boustrophedon_explorator.h，接口定义如下：

	// divides the provided map into Morse cells
	void computeCellDecomposition(const cv::Mat& room_map, const float map_resolution, const double min_cell_area,
			const int min_cell_width, std::vector<GeneralizedPolygon>& cell_polygons, std::vector<cv::Point>& polygon_centers);

其中，room_map是经过预处理以及最佳角度旋转后的栅格地图，该地图是一张二值图，0代表障碍物，255代表可通行区域；map_resolution是栅格地图的分辨率；min_cell_area用来过滤小面积区域；min_cell_width用来过滤窄区域；cell_polygons保存返回的分区结果，生成的区间以多边形点的形式保存；polygon_centers保存每一个分区的中心点坐标，用来计算区间之间的距离。

该函数的主要分为两个步骤：第一步是使用牛耕法将地图分割为若干个子区间；第二步则是对第一步分割好的区间做一个合并操作，将小面积区间合并到相邻面积最大的区间。

网上已经有不少相关资料详细介绍了牛耕法，故对牛耕法的原理部分就不过多展开，有兴趣的可以去看这篇文章：Exact cellular decompositions in terms of critical points of Morse functions。

牛耕法会逐行（或逐列）计算该行（或该列）被障碍物分为几段，根据前后连续两行（或两列）的段数，会触发不同的处理逻辑：

1.当前段数比上一个的段数多，则触发IN事件处理。

2.当前段数比上一个的段数少，则触发OUT事件处理。

3.当前段跟上一个的段数一样，但不连续，则同样当IN事件处理。

ROS里面这部分源码是按照从上到下逐行遍历分割处理的。其整体的流程图如下所示：

下面通过分析里面的一段代码来帮助理解，代码如下:

	// find smallest y-value for that a white pixel occurs, to set initial y value and find initial number of segments
	size_t y_start = 0;
	bool found = false, obstacle = false;
	int previous_number_of_segments = 0;
	std::vector<int> previous_obstacles_end_x;		// keep track of the end points of obstacles
	for(size_t y=0; y<room_map.rows; ++y)
	{
		for(size_t x=0; x<room_map.cols; ++x)
		{
			if(found == false && room_map.at<uchar>(y,x) == 255)
			{
				y_start = y;
				found = true;
			}
			else if(found == true && obstacle == false && room_map.at<uchar>(y,x) == 0)
			{
				++previous_number_of_segments;
				obstacle = true;
			}
			else if(found == true && obstacle == true && room_map.at<uchar>(y,x) == 255)
			{
				obstacle = false;
				previous_obstacles_end_x.push_back(x);
			}
		}
		if(found == true)
			break;
	}

这段代码的功能是从上到下，从左到右，遍历地图数据，当找到房间的第一行时，记录下当时的Y坐标y_start，以及该行的分段数previous_number_of_segments和障碍点的X坐标列表previous_obstacles_end_x。为后续进一步做计算做好准备。

当输入的地图数据比较复杂时，可能会生成比较多的小区间，这时候就要对上一步生成的区间进行合并。ROS定义了一种数据结构保存每一个区间的信息，代码如下：

// Structure for saving several properties of cells
struct BoustrophedonCell
{
	typedef std::set<boost::shared_ptr<BoustrophedonCell> > BoustrophedonCellSet;
	typedef std::set<boost::shared_ptr<BoustrophedonCell> >::iterator BoustrophedonCellSetIterator;

	int label_;				// label id of the cell
	double area_;			// area of the cell, in [pixel^2]
	cv::Rect bounding_box_;		// bounding box of the cell
	BoustrophedonCellSet neighbors_;		// pointer to neighboring cells

	BoustrophedonCell(const int label, const double area, const cv::Rect& bounding_box)
	{
		label_ = label;
		area_ = area;
		bounding_box_ = bounding_box;
	}

};

其中，label_的值用来表示不同区间，area_表示区间的面积大小，neighbors_则使用set的方式存储该区间相邻的区间。当看到有label_跟area_的时候，有过opencv图像处理经验的人可能就已经想到ROS是使用水漫法来做区间分离了。ROS里面给出的代码实现也比较简单，就不过多展开描述了，主要关键的地方了解两个就行：第一是使用了multimap这种数据结构来保存面积与单元相邻区间的信息area_to_region_id_mapping，key为面积，value为区间；第二是合并的一个原则是将面积小于最小面积或过窄的区间合并到相邻面积最大的区间。

至此，就完成了区间的分割合并操作，并将结果输入给下一步做具体的遍历顺序计算以及弓字形覆盖路径生成。