ipa_coverage_planning 算法解读（四） RoomRotator::computeRoomMainDirection

RoomRotator::computeRoomMainDirection

作为比较单独且核心的一个算法，这里单独拿出来方便后续查看

 复制代码
double RoomRotator::computeRoomMainDirection(const cv::Mat& room_map, const double map_resolution)
{
	const double map_resolution_inverse = 1./map_resolution;
 
	// compute Hough transform on edge image of the map
	cv::Mat edge_map;
	cv::Canny(room_map, edge_map, 50, 150, 3);
	std::vector<cv::Vec4i> lines;
	double min_line_length = 1.0;	// in [m]
	for (; min_line_length > 0.1; min_line_length -= 0.2)
	{
		cv::HoughLinesP(edge_map, lines, 1, CV_PI/180, min_line_length*map_resolution_inverse, min_line_length*map_resolution_inverse, 1.5*min_line_length*map_resolution_inverse);
		cv::Mat room_hough = edge_map.clone();
		for (size_t i=0; i<lines.size(); ++i)
		{
			cv::Point p1(lines[i][0], lines[i][1]), p2(lines[i][2], lines[i][3]);
			cv::line(room_hough, p1, p2, cv::Scalar(128), 2);
		}
		//cv::imshow("room_hough", room_hough);
		if (lines.size() >= 4)
			break;
	}
	// setup a histogram on the line directions weighted by their length to determine the major direction
	Histogram<double> direction_histogram(0, CV_PI, 36);
	for (size_t i=0; i<lines.size(); ++i)
	{
		double dx = lines[i][2] - lines[i][0];
		double dy = lines[i][3] - lines[i][1];
		if(dy*dy+dx*dx > 0.0)
		{
			double current_direction = std::atan2(dy, dx);
			while (current_direction < 0.)
				current_direction += CV_PI;
			while (current_direction > CV_PI)
				current_direction -= CV_PI;
			direction_histogram.addData(current_direction, sqrt(dy*dy+dx*dx));
			//std::cout << " dx=" << dx << "   dy=" << dy << "   dir=" << current_direction << "   len=" << sqrt(dy*dy+dx*dx) << std::endl;
		}
	}
	return direction_histogram.getMaxBinPreciseVal();
}

详细解释：
计算分辨率的倒数，用于长度单位转换
const double map_resolution_inverse = 1./map_resolution;

使用 cv::Canny 函数对房间地图进行边缘检测，得到边缘图 edge_map
cv::Mat edge_map;
cv::Canny(room_map, edge_map, 50, 150, 3);


使用霍夫变化检测直线

 复制代码
std::vector<cv::Vec4i> lines;
double min_line_length = 1.0;	// in [m]
for (; min_line_length > 0.1; min_line_length -= 0.2)
{
	cv::HoughLinesP(edge_map, lines, 1, CV_PI/180, min_line_length*map_resolution_inverse, min_line_length*map_resolution_inverse, 1.5*min_line_length*map_resolution_inverse);
	cv::Mat room_hough = edge_map.clone();
	for (size_t i=0; i<lines.size(); ++i)
	{
		cv::Point p1(lines[i][0], lines[i][1]), p2(lines[i][2], lines[i][3]);
		cv::line(room_hough, p1, p2, cv::Scalar(128), 2);
	}
	//cv::imshow("room_hough", room_hough);
	if (lines.size() >= 4)
		break;
}

使用 Histogram 类构建一个直方图，记录所有检测到的直线的方向和长度。
Histogram direction_histogram(0, CV_PI, 36);

遍历所有直线，计算它们的方向，并更新方向直方图。最后，返回方向直方图中最大值对应的方向，即为房间地图的主要方向。

 复制代码
for (size_t i=0; i<lines.size(); ++i)
{
	double dx = lines[i][2] - lines[i][0];
	double dy = lines[i][3] - lines[i][1];
	if(dy*dy+dx*dx > 0.0)
	{
		double current_direction = std::atan2(dy, dx);
		while (current_direction < 0.)
			current_direction += CV_PI;
		while (current_direction > CV_PI)
			current_direction -= CV_PI;
		direction_histogram.addData(current_direction, sqrt(dy*dy+dx*dx));
		//std::cout << " dx=" << dx << "   dy=" << dy << "   dir=" << current_direction << "   len=" << sqrt(dy*dy+dx*dx) << std::endl;
	}
}
return direction_histogram.getMaxBinPreciseVal();