批量计算地图弓字路径

分析：

对每个cell都会计算旋转方向；cell之间的路径直接使用直线连接起来的；

另外有一些操作值得借鉴，例如对轮廓的搜索，小的cell的合并和去除；

去除边缘噪点，纠正断断续续的墙；

void BoustrophedonExplorer::correctThinWalls(cv::Mat& room_map)
{
	for (int v=1; v<room_map.rows; ++v)
	{
		for (int u=1; u<room_map.cols; ++u)
		{
			if (room_map.at<uchar>(v-1,u-1)==255 && room_map.at<uchar>(v-1,u)==0 && room_map.at<uchar>(v,u-1)==0 && room_map.at<uchar>(v,u)==255)
				room_map.at<uchar>(v,u)=0;
			else if (room_map.at<uchar>(v-1,u-1)==0 && room_map.at<uchar>(v-1,u)==255 && room_map.at<uchar>(v,u-1)==255 && room_map.at<uchar>(v,u)==0)
				room_map.at<uchar>(v,u-1)=0;
		}
	}
}

路径稀疏化

void BoustrophedonExplorer::downsamplePath(const std::vector<cv::Point>& original_path, std::vector<cv::Point>& downsampled_path,
		cv::Point& robot_pos, const double path_eps)
{
	// downsample path
	for(size_t path_point=0; path_point<original_path.size(); ++path_point)
	{
		if(cv::norm(robot_pos-original_path[path_point]) >= path_eps)
		{
			downsampled_path.push_back(original_path[path_point]);
			robot_pos = original_path[path_point];
		}
	}
	// add last element
	if (original_path.size() > 0)
	{
		downsampled_path.push_back(original_path.back());
		robot_pos = original_path.back();
	}
}

将多边形填充绘制到图片上

void drawPolygon(cv::Mat& image, const cv::Scalar& color) const
	{
		// draw polygon in an black image with necessary size
		cv::Mat black_image = cv::Mat(max_y_+10, max_x_+10, CV_8UC1, cv::Scalar(0));
#if CV_MAJOR_VERSION<=3
		cv::drawContours(black_image, std::vector<std::vector<cv::Point> >(1,vertices_), -1, color, CV_FILLED);
#else
		cv::drawContours(black_image, std::vector<std::vector<cv::Point> >(1,vertices_), -1, color, cv::FILLED);
#endif

		// assign drawn map
		image = black_image.clone();
	}