ipa_coverage_planning 算法解读（三）

书接上文，本文主要介绍牛耕（弓字型）的代码

getExplorationPath函数接口定义如下：

void BoustrophedonExplorer::getExplorationPath(const cv::Mat& room_map, std::vector<geometry_msgs::Pose2D>& path,
		const float map_resolution, const cv::Point starting_position, const cv::Point2d map_origin,
		const double grid_spacing_in_pixel, const double grid_obstacle_offset, const double path_eps, const int cell_visiting_order,
		const bool plan_for_footprint, const Eigen::Matrix<float, 2, 1> robot_to_fov_vector, const double min_cell_area, const int max_deviation_from_track)

getExplorationPath函数的参数说明：
const cv::Mat& room_map: 输入的房间地图，通常是一个二维矩阵，表示房间的布局。

std::vector& path: 输出的探索路径，存储了机器人在房间中探索时的位置和方向。

const float map_resolution: 地图的分辨率，即每个像素或单元格代表的实际面积大小。

const cv::Point starting_position: 探索的起始位置。

const cv::Point2d map_origin: 地图的原点坐标。

const double grid_spacing_in_pixel: 网格间距（以像素为单位）。

const double grid_obstacle_offset: 网格障碍物偏移量。

const double path_eps: 路径精度参数。

const int cell_visiting_order: 单元格访问顺序，可以是最优的旅行商问题（TSP）解决方案或简单的从左到右的顺序。

const bool plan_for_footprint: 是否为机器人的实际占用空间（footprint）规划路径。

const Eigen::Matrix robot_to_fov_vector: 机器人视野中心到机器人实际位置的向量。

const double min_cell_area: 最小单元格面积。

const int max_deviation_from_track: 最大偏离轨迹的程度。

getExplorationPath函数的操作流程包括：
1.计算房间地图的主要方向，并根据这个方向和额外的旋转偏移量来确定旋转角度。
2.确定旋转中心，如果没有提供，则根据房间地图的最小和最大坐标点来确定。
3.计算旋转矩阵，使用OpenCV的 cv::getRotationMatrix2D 函数。
4.确定旋转后的边界矩形，以便知道新图像的大小。
5.调整变换矩阵，以确保旋转后的图像正确对齐。
6.返回旋转角度。

1> grid_spacing_as_int: 将 grid_spacing_in_pixel（网格间距，以像素为单位）转换为整数。这是通过取其下限（std::floor）来实现的，确保网格间距是一个整数值。
2> half_grid_spacing_as_int: 计算网格间距的一半，并将其转换为整数。这个值在后续的计算中可能用于确定单元格的边界或者是用于路径规划时考虑的安全边距。
3> min_cell_width: 计算最小单元格宽度。这是通过将 half_grid_spacing_as_int（即网格间距的一半）加上两倍的 grid_obstacle_offset（网格障碍物偏移量）除以 map_resolution（地图分辨率）来实现的。这个值用于确定在探索路径规划中每个单元格的最小尺寸，以确保机器人在探索时不会太靠近障碍物。

const int grid_spacing_as_int = (int)std::floor(grid_spacing_in_pixel); // convert fov-radius to int
const int half_grid_spacing_as_int = (int)std::floor(0.5*grid_spacing_in_pixel); // convert fov-radius to int
const int min_cell_width = half_grid_spacing_as_int + 2.*grid_obstacle_offset/map_resolution;

1> cv::Mat R;: 声明一个 cv::Mat 类型的变量 R，用于存储旋转矩阵。
2> cv::Rect bbox;: 声明一个 cv::Rect 类型的变量 bbox，用于存储旋转后的房间地图的边界矩形。
3> cv::Mat rotated_room_map;: 声明一个 cv::Mat 类型的变量 rotated_room_map，用于存储旋转后的房间地图。
4> std::vector cell_polygons;: 声明一个 GeneralizedPolygon 类型的向量 cell_polygons，用于存储房间地图中每个单元格的多边形表示。
5> std::vector polygon_centers;: 声明一个 cv::Point 类型的向量 polygon_centers，用于存储每个单元格多边形的中心点。
6> computeCellDecompositionWithRotation(...): 调用 computeCellDecompositionWithRotation 函数，传入房间地图、地图分辨率、最小单元格面积、最小单元格宽度等参数，以及之前声明的变量。这个函数的目的是计算出房间地图的单元格分解，包括每个单元格的多边形表示和中心点，同时返回旋转矩阵 R 和边界矩形 bbox。

cv::Mat R;
cv::Rect bbox;
cv::Mat rotated_room_map;
std::vector<GeneralizedPolygon> cell_polygons;
std::vector<cv::Point> polygon_centers;
computeCellDecompositionWithRotation(room_map, map_resolution, min_cell_area, min_cell_width, 0., R, bbox, rotated_room_map, cell_polygons, polygon_centers);

computeCellDecompositionWithRotation
1> RoomRotator room_rotation;: 创建一个 RoomRotator 类的实例，用于处理房间地图的旋转。
2> room_rotation.computeRoomRotationMatrix(...): 调用 RoomRotator 类的 computeRoomRotationMatrix 方法，计算出旋转矩阵 R 和边界矩形 bbox。这个方法根据房间地图的主要方向和旋转偏移量来确定旋转角度。
3> room_rotation.rotateRoom(...): 使用计算出的旋转矩阵 R 和边界矩形 bbox 来旋转房间地图，得到旋转后的房间地图 rotated_room_map。
4> computeCellDecomposition(...): 调用 computeCellDecomposition 函数，传入旋转后的房间地图、地图分辨率、最小单元格面积和最小单元格宽度等参数，以及之前声明的变量。这个函数的目的是计算出旋转后房间地图的单元格分解，包括每个单元格的多边形表示和中心点。

void BoustrophedonExplorer::computeCellDecompositionWithRotation(const cv::Mat& room_map, const float map_resolution, const double min_cell_area,
		const int min_cell_width, const double rotation_offset, cv::Mat& R, cv::Rect& bbox, cv::Mat& rotated_room_map,
		std::vector<GeneralizedPolygon>& cell_polygons, std::vector<cv::Point>& polygon_centers)
{
	RoomRotator room_rotation;
	room_rotation.computeRoomRotationMatrix(room_map, R, bbox, map_resolution, 0, rotation_offset);
	room_rotation.rotateRoom(room_map, rotated_room_map, R, bbox);
	computeCellDecomposition(rotated_room_map, map_resolution, min_cell_area, min_cell_width, cell_polygons, polygon_centers);
}

1> 计算旋转角度：首先，通过 computeRoomMainDirection 函数计算出房间地图的主要方向，并加上一个旋转偏移量 rotation_offset，得到旋转角度 rotation_angle。
2> 确定旋转中心：如果没有提供旋转中心（即 center 指针为空），则通过 getMinMaxCoordinates 函数找到房间地图的最小和最大坐标点，计算出旋转中心 center_of_rotation。如果提供了旋转中心，则直接使用该中心。
3> 计算旋转矩阵：使用 OpenCV 的 cv::getRotationMatrix2D 函数，根据旋转中心、旋转角度（转换为度数）和缩放因子（这里为 1.0）来计算旋转矩阵 R。
4> 确定旋转后的边界矩形：计算旋转后的房间地图的边界矩形 bounding_rect，这是为了确定新图像的大小。
5> 调整变换矩阵：调整旋转矩阵 R，以确保旋转后的图像正确对齐。这涉及到调整 R 矩阵的第二行和第三行，以考虑边界矩形的大小和位置。
6> 返回旋转角度：最后，函数返回计算出的旋转角度 rotation_angle。

double RoomRotator::computeRoomRotationMatrix(const cv::Mat& room_map, cv::Mat& R, cv::Rect& bounding_rect,
		const double map_resolution, const cv::Point* center, const double rotation_offset)
{
	// rotation angle of the map s.t. the most occurring gradient is in 90 degree to the x-axis
	double rotation_angle = computeRoomMainDirection(room_map, map_resolution) + rotation_offset;
	std::cout << "RoomRotator::computeRoomRotationMatrix: main rotation angle: " << rotation_angle << std::endl;

	// get rotation matrix R for rotating the image around the center of the room contour
	//	Remark: rotation angle in degrees for opencv
	cv::Point center_of_rotation;
	if (center == 0)
	{
		cv::Point min_room, max_room;
		getMinMaxCoordinates(room_map, min_room, max_room);
		center_of_rotation.x = 0.5*(min_room.x+max_room.x);
		center_of_rotation.y = 0.5*(min_room.y+max_room.y);
	}
	else
		center_of_rotation = *center;

	// compute rotation
	R = cv::getRotationMatrix2D(center_of_rotation, (rotation_angle*180)/CV_PI, 1.0);

	// determine bounding rectangle to find the size of the new image
	bounding_rect = cv::RotatedRect(center_of_rotation, room_map.size(), (rotation_angle*180)/CV_PI).boundingRect();
	// adjust transformation matrix
	R.at<double>(0,2) += 0.5*bounding_rect.width - center_of_rotation.x;
	R.at<double>(1,2) += 0.5*bounding_rect.height - center_of_rotation.y;

	return rotation_angle;
}