书接上文：牛耕分解+形态学分割 全覆盖路径规划（二）Part1. 分割
在上文中我们单独对“分区”做了较为详细的了解，本文中，我们对“覆盖”再做探究
先放结果：
算法作者给的注释，大致的给出了整个算法的工作流程：
I.Using the Sobel operator the direction of the gradient at each pixel is computed. Using this information, the direction is found that suits best for calculating the cells, i.e. such that longer cells occur, and the map is rotated in this manner. This allows to use the algorithm as it was and in the last step, the found path points simply will be transformed back to the original orientation.
II.Sweep a slice (a morse function) trough the given map and check for connectivity of this line, i.e. how many connected segments there are. If the connectivity increases, i.e. more segments appear, an IN event occurs that opens new separate cells, if it decreases, i.e. segments merge, an OUT event occurs that merges two cells together. If an event occurs, the algorithm checks along the current line for critical points, that are points that trigger the events. From these the boundary of the cells are drawn, starting from the CP and going left/right until a black pixel is hit.
III.After all cells have been determined by the sweeping slice, the algorithm finds these by using cv::findContours(). This gives a set of points for each cell, that are used to create a generalizedPolygon out of each cell.
IV.After all polygons have been created, plan the path trough all of them for the field of view s.t. the whole area is covered. To do so, first a global path trough all cells is generated, using the traveling salesmen problem formulation. This produces an optimal visiting order of the cells. Next for each cell a boustrophedon path is determined, which goes back and forth trough the cell and between the horizontal paths along the boundaries of the cell, what ensures that the whole area of the cell is covered. For each cell the longest edge is found and it is transformed s.t. this edge lies horizontal to the x-direction. This produces longer but fewer edges, what improves the path for small but long cells. The startpoint of the cell-path is determined by the endpoint of the previous cell, s.t. the distance between two cell-paths is minimized. The cell-path is determined in the rotated manner, so in a last step, the cell-path is transformed to the originally transformed cell and after that inserted in the global path.
V.The previous step produces a path for the field of view. If wanted this path gets mapped to the robot path s.t. the field of view follows the wanted path. To do so simply a vector transformation is applied. If the computed robot pose is not in the free space, another accessible point is generated by finding it on the radius around the field of view (fov) middlepoint s.t. the distance to the last robot position is minimized. If this is not wanted one has to set the corresponding Boolean to false (shows that the path planning should be done for the robot footprint).
程序中的执行逻辑：
- I. Find the main directions of the map and rotate it in this manner.
- II. Sweep a slice trough the map and mark the found cell boundaries.
- III. Find the separated cells.
- IV. Determine the cell paths.
- V. Get the robot path out of the fov path.
从代码实现来看，先看调用：
boustrophedon_variant_explorer_.getExplorationPath(room_map, exploration_path, map_resolution_, starting_position_, map_origin_, grid_spacing_in_pixel, grid_obstacle_offset_, path_eps_, cell_visiting_order_, true, zero_vector, min_cell_area_, max_deviation_from_track_);使用 getExplorationPath 函数时需要先准备好以下参数：
-
room_map: cv::Mat 类型矩阵，表示房间地图 -
exploration_path: 这是一个向量，用于存储计算出的探索路径。每个元素是一个geometry_msgs::Pose2D对象，表示机器人在探索过程中的位置和方向。 -
map_resolution_: 表示地图的分辨率，即地图上每个像素代表的实际距离。这个参数用于将地图上的尺寸转换为实际的物理尺寸。 -
starting_position_:cv::Point对象，表示机器人的初始位置。 -
map_origin_: 地图原点坐标，在MapCallback中给出，一般在nav_msgs::OccupancyGrid消息类型中的info.origin中给出。 -
grid_spacing_in_pixel: 这是网格的像素间距。它是基于机器人覆盖半径计算出的，用于确定探索网格的大小。 -
grid_obstacle_offset_: 这是网格中障碍物的偏移量。它用于调整障碍物在网格中的位置，以便更准确地反映实际障碍物的位置。 -
path_eps_: 这是路径精度参数。它决定了路径规划的精细程度，较小的值意味着更精确的路径。 -
cell_visiting_order_: 在该算法中，用于判断使用TSP规划器还是自左向右规划期进行规划 -
zero_vector: 这是一个二维零向量，通常用于初始化或作为默认参数传递。 -
min_cell_area_: 最小分区面积，用在分割关键点区域后，进行多个区域合并 -
max_deviation_from_track_: 这是机器人从预定路径上最大的偏离量。它用于确保机器人在探索过程中不会偏离太远。
Step 1 寻找地图的主要方向并旋转地图
主要调用 RoomRotator 进行实现
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);第一步 computeRoomRotationMatrix 确定房间的主要方向，并返回旋转矩阵R；在这里说明一下，参数rotation_offset是可选的旋转偏移量，由于传入为 0. ，所以不设置旋转偏移。computeRoomRotationMatrix的实现如下：
// compute the affine rotation matrix for rotating a room into parallel alignment with x-axis (longer side of the room is aligned with x-axis)
// R is the transform
// bounding_rect is the ROI of the warped image
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;
}该函数可以概括为这么几步：
计算旋转角度->确定旋转中心->计算旋转矩阵->调整旋转矩阵
1.1 计算旋转角度 computeRoomMainDirection
// computes the major direction of the walls from a map (preferably one room)
// the map (room_map, CV_8UC1) is black (0) at impassable areas and white (255) on drivable areas
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();
}① 通过OpenCV的 Canny边缘检测算法识别房间地图的边缘；
② 对边缘图像edge_map应用霍夫变换检测直线，将直线段存储在std::vector<cv::Vec4i> lines中，通过逐渐减小直线长度的最小阈值min_line_length尝试找到足够长的直线段，从而代表房间的额主要结构；
③ 设置一个方向直方图Histogram<double> direction_histogram用于统计所有直线段的方向，每个直线段的方向通过其两端点的差分计算得出，并加权其长度，以确保较长的直线段对最终方向的贡献更大；
④ 函数通过查找方向直方图中的最大值来确定房间的主要方向。
1.2 确定旋转中心
// 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;这里的getMinMaxCoordinates方法是获取房间中心点，具体实现如下：
void RoomRotator::getMinMaxCoordinates(const cv::Mat& map, cv::Point& min_room, cv::Point& max_room)
{
min_room.x = std::numeric_limits<int>::max();
min_room.y = std::numeric_limits<int>::max();
max_room.x = 0;
max_room.y = 0;
for (int v=0; v<map.rows; ++v)
{
for (int u=0; u<map.cols; ++u)
{
if (map.at<uchar>(v,u)==255)
{
min_room.x = std::min(min_room.x, u);
min_room.y = std::min(min_room.y, v);
max_room.x = std::max(max_room.x, u);
max_room.y = std::max(max_room.y, v);
}
}
}
}1.3 计算旋转矩阵
R = cv::getRotationMatrix2D(center_of_rotation, (rotation_angle*180)/CV_PI, 1.0);调用了OpenCV的getRotationMatrix2D方法
1.4 调整旋转矩阵
// 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;为了确保旋转后的图像完全位于视图框内，函数计算了旋转后图像的边界矩形bounding_rect，并相应地调整旋转矩阵的平移部分。这一步确保旋转后的图像不会被裁剪。
第二步 rotateRoom 根据第一步计算好的旋转矩阵R和边界矩形bounding_rect对房间进行旋转
void RoomRotator::rotateRoom(const cv::Mat& room_map, cv::Mat& rotated_room_map, const cv::Mat& R, const cv::Rect& bounding_rect)
{
// rotate the image
cv::warpAffine(room_map, rotated_room_map, R, bounding_rect.size(), cv::INTER_AREA);
// apply a binary filter to create a binary image, also use a closing operator to smooth the output (the rotation might produce
// black pixels reaching into the white area that were not there before, causing new, wrong cells to open)
#if CV_MAJOR_VERSION<=3
cv::threshold(rotated_room_map, rotated_room_map, 127, 255, CV_THRESH_BINARY);
#else
cv::threshold(rotated_room_map, rotated_room_map, 127, 255, cv::THRESH_BINARY);
#endif
// this should not be used because it removes smaller obstacles like thin walls from the room and hence lets a planner generate paths through walls
// cv::dilate(rotated_room_map, rotated_room_map, cv::Mat(), cv::Point(-1,-1), 1);
// cv::erode(rotated_room_map, rotated_room_map, cv::Mat(), cv::Point(-1,-1), 1);
}rotateRoom进行了两步操作，图像旋转和二值化，没什么特别的；作者在这里注释掉了腐蚀膨胀操作，表明可能腐蚀膨胀后会出现穿墙的路径。
Step 2 分割地图
void BoustrophedonExplorer::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)
{
// *********************** II. Sweep a slice trough the map and mark the found cell boundaries. ***********************
// create a map copy to mark the cell boundaries
cv::Mat cell_map = room_map.clone();
// 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;
}
// sweep trough the map and detect critical points
for(size_t y=y_start+1; y<room_map.rows; ++y) // start at y_start+1 because we know number of segments at y_start
{
int number_of_segments = 0; // int to count how many segments at the current slice are
std::vector<int> current_obstacles_start_x;
std::vector<int> current_obstacles_end_x;
bool obstacle_hit = false; // bool to check if the line currently hit an obstacle, s.t. not all black pixels trigger an event
bool hit_white_pixel = false; // bool to check if a white pixel has been hit at the current slice, to start the slice at the first white pixel
// count number of segments within this row
for(size_t x=0; x<room_map.cols; ++x)
{
if(hit_white_pixel == false && room_map.at<uchar>(y,x) == 255)
hit_white_pixel = true;
else if(hit_white_pixel == true)
{
if(obstacle_hit == false && room_map.at<uchar>(y,x) == 0) // check for obstacle
{
++number_of_segments;
obstacle_hit = true;
current_obstacles_start_x.push_back(x);
}
else if(obstacle_hit == true && room_map.at<uchar>(y,x) == 255) // check for leaving obstacle
{
obstacle_hit = false;
current_obstacles_end_x.push_back(x);
}
}
}
// if the number of segments did not change, check whether the position of segments has changed so that there is a gap between them
bool segment_shift_detected = false;
if (previous_number_of_segments == number_of_segments && current_obstacles_start_x.size() == previous_obstacles_end_x.size()+1)
{
for (size_t i=0; i<previous_obstacles_end_x.size(); ++i)
if (current_obstacles_start_x[i] > previous_obstacles_end_x[i])
{
segment_shift_detected = true;
break;
}
}
// reset hit_white_pixel to use this Boolean later
hit_white_pixel = false;
// check if number of segments has changed --> event occurred
if(previous_number_of_segments < number_of_segments || segment_shift_detected == true) // IN event (or shift)
{
// check the current slice again for critical points
for(int x=0; x<room_map.cols; ++x)
{
if(hit_white_pixel == false && room_map.at<uchar>(y,x) == 255)
hit_white_pixel = true;
else if(hit_white_pixel == true && room_map.at<uchar>(y,x) == 0)
{
// check over black pixel for other black pixels, if none occur a critical point is found
bool critical_point = true;
for(int dx=-1; dx<=1; ++dx)
if(room_map.at<uchar>(y-1,std::max(0,std::min(x+dx, room_map.cols-1))) == 0)
critical_point = false;
// if a critical point is found mark the separation, note that this algorithm goes left and right
// starting at the critical point until an obstacle is hit, because this prevents unnecessary cells
// behind other obstacles on the same y-value as the critical point
if(critical_point == true)
{
// to the left until a black pixel is hit
for(int dx=-1; x+dx>=0; --dx)
{
uchar& val = cell_map.at<uchar>(y,x+dx);
if(val == 255 && cell_map.at<uchar>(y-1,x+dx) == 255)
val = BORDER_PIXEL_VALUE;
else if(val == 0)
break;
}
// to the right until a black pixel is hit
for(int dx=1; x+dx<room_map.cols; ++dx)
{
uchar& val = cell_map.at<uchar>(y,x+dx);
if(val == 255 && cell_map.at<uchar>(y-1,x+dx) == 255)
val = BORDER_PIXEL_VALUE;
else if(val == 0)
break;
}
}
}
}
}
else if(previous_number_of_segments > number_of_segments) // OUT event
{
// check the previous slice again for critical points --> y-1
for(int x=0; x<room_map.cols; ++x)
{
if(room_map.at<uchar>(y-1,x) == 255 && hit_white_pixel == false)
hit_white_pixel = true;
else if(hit_white_pixel == true && room_map.at<uchar>(y-1,x) == 0)
{
// check over black pixel for other black pixels, if none occur a critical point is found
bool critical_point = true;
for(int dx=-1; dx<=1; ++dx)
if(room_map.at<uchar>(y,std::max(0,std::min(x+dx, room_map.cols-1))) == 0) // check at side after obstacle
critical_point = false;
// if a critical point is found mark the separation, note that this algorithm goes left and right
// starting at the critical point until an obstacle is hit, because this prevents unnecessary cells
// behind other obstacles on the same y-value as the critical point
if(critical_point == true)
{
const int ym2 = std::max(0,(int)y-2);
// to the left until a black pixel is hit
for(int dx=-1; x+dx>=0; --dx)
{
uchar& val = cell_map.at<uchar>(y-1,x+dx);
if(val == 255 && cell_map.at<uchar>(ym2,x+dx) == 255)
val = BORDER_PIXEL_VALUE;
else if(val == 0)
break;
}
// to the right until a black pixel is hit
for(int dx=1; x+dx<room_map.cols; ++dx)
{
uchar& val = cell_map.at<uchar>(y-1,x+dx);
if(val == 255 && cell_map.at<uchar>(ym2,x+dx) == 255)
val = BORDER_PIXEL_VALUE;
else if(val == 0)
break;
}
}
}
}
}
// save the found number of segments and the obstacle end points
previous_number_of_segments = number_of_segments;
previous_obstacles_end_x = current_obstacles_end_x;
}
#ifdef DEBUG_VISUALIZATION
cv::imshow("cell_map", cell_map);
#endif
// *********************** II.b) merge too small cells into bigger cells ***********************
cv::Mat cell_map_labels;
const int number_of_cells = mergeCells(cell_map, cell_map_labels, min_cell_area, min_cell_width);
// *********************** III. Find the separated cells. ***********************
std::vector<std::vector<cv::Point> > cells;
for (int i=1; i<=number_of_cells; ++i)
{
cv::Mat cell_copy(cell_map_labels == i);
std::vector<std::vector<cv::Point> > cellsi;
#if CV_MAJOR_VERSION<=3
cv::findContours(cell_copy, cellsi, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
#else
cv::findContours(cell_copy, cellsi, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
#endif
cells.insert(cells.end(), cellsi.begin(), cellsi.end());
}
#ifdef DEBUG_VISUALIZATION
// // testing
// cv::Mat black_map = cv::Mat::zeros(cell_map.rows, cell_map.cols, cell_map.type());
// for(size_t i=0; i<cells.size(); ++i)
// {
// cv::drawContours(black_map, cells, i, cv::Scalar(127), cv::FILLED);
// cv::imshow("contours", black_map);
// cv::waitKey();
// }
#endif
// create generalized Polygons out of the contours to handle the cells
for(size_t cell=0; cell<cells.size(); ++cell)
{
GeneralizedPolygon current_cell(cells[cell], map_resolution);
// std::cout << cell+1 << ": " << current_cell.getArea() << std::endl;
if(current_cell.getArea()>=min_cell_area)
{
cell_polygons.push_back(current_cell);
polygon_centers.push_back(current_cell.getCenter());
}
else
{
std::cout << "WARN: BoustrophedonExplorer::computeCellDecomposition: dropped cell " << cell+1 << " with area=" << current_cell.getArea() << ". This should only happen for small unconnected cells." << std::endl;
}
}
}这一部分较为庞大，我们尽可能的不留疑惑去分析
2.1 查找第一个可通行区域位置
// 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;
}逻辑：遍历地图寻找可通行区域，找到后found置为true；在该行中继续查找，如果found为true且当前像素值为0（黑色），则表示进入了一个障碍物。此时，增加previous_number_of_segments的值，并将obstacle标志设置为true；如果found为true且obstacle为true且当前像素值为255，则表示离开了障碍物。此时，将obstacle标志设置为false，并将当前的x坐标添加到previous_obstacles_end_x中；在找到可通过区域后，检索完该行后跳出外层循环。
2.2 检测关键点
for(size_t y=y_start+1; y<room_map.rows; ++y)
{
…………
}这段代码用于在房间地图上扫描并检测关键点（critical points），由于整个for中的步骤过多，所以拆分为多个步骤进行研究。
2.2.1 计算当前行的障碍物段数
// count number of segments within this row
for(size_t x=0; x<room_map.cols; ++x)
{
if(hit_white_pixel == false && room_map.at<uchar>(y,x) == 255)
hit_white_pixel = true;
else if(hit_white_pixel == true)
{
if(obstacle_hit == false && room_map.at<uchar>(y,x) == 0)
{
++number_of_segments;
obstacle_hit = true;
current_obstacles_start_x.push_back(x);
}
else if(obstacle_hit == true && room_map.at<uchar>(y,x) == 255)
{
obstacle_hit = false;
current_obstacles_end_x.push_back(x);
}
}
}通过遍历当前行的每个像素，根据像素值（0表示黑色，255表示白色）来判断障碍物的开始和结束，从而计算出当前行的障碍物段数。
2.2.2 检测段数变化或段位移
// if the number of segments did not change, check whether the position of segments has changed so that there is a gap between them
bool segment_shift_detected = false;
if (previous_number_of_segments == number_of_segments && current_obstacles_start_x.size() == previous_obstacles_end_x.size()+1)
{
for (size_t i=0; i<previous_obstacles_end_x.size(); ++i)
if (current_obstacles_start_x[i] > previous_obstacles_end_x[i])
{
segment_shift_detected = true;
break;
}
}
// reset hit_white_pixel to use this Boolean later
hit_white_pixel = false;
// check if number of segments has changed --> event occurred
if(previous_number_of_segments < number_of_segments || segment_shift_detected == true) // IN event (or shift)
{
……
}
else if(previous_number_of_segments > number_of_segments) // OUT event
{
……
}首先检查是否发生了“IN事件”（即障碍物段数增加或段位移）或“OUT事件”（即障碍物段数减少）。这些事件表明房间地图的结构发生了变化，需要重新检测关键点。
2.2.3 检测关键点
// check over black pixel for other black pixels, if none occur a critical point is found
bool critical_point = true;
for(int dx=-1; dx<=1; ++dx)
if(room_map.at<uchar>(y-1,std::max(0,std::min(x+dx, room_map.cols-1))) == 0)
critical_point = false;这里需要区分“IN”事件还是“OUT”事件
对于“IN事件”，从第一个白色像素开始，检查每个黑色像素周围是否没有其他黑色像素。如果是这样，那么当前的黑色像素就是一个关键点。
对于“OUT事件”，同样检查黑色像素周围是否没有其他黑色像素，但是这次是在前一行的基础上进行检查。
2.2.4 标记关键点
// if a critical point is found mark the separation, note that this algorithm goes left and right
// starting at the critical point until an obstacle is hit, because this prevents unnecessary cells
// behind other obstacles on the same y-value as the critical point
if(critical_point == true)
{
// to the left until a black pixel is hit
for(int dx=-1; x+dx>=0; --dx)
{
uchar& val = cell_map.at<uchar>(y,x+dx);
if(val == 255 && cell_map.at<uchar>(y-1,x+dx) == 255)
val = BORDER_PIXEL_VALUE;
else if(val == 0)
break;
}
// to the right until a black pixel is hit
for(int dx=1; x+dx<room_map.cols; ++dx)
{
uchar& val = cell_map.at<uchar>(y,x+dx);
if(val == 255 && cell_map.at<uchar>(y-1,x+dx) == 255)
val = BORDER_PIXEL_VALUE;
else if(val == 0)
break;
}
}// if a critical point is found mark the separation, note that this algorithm goes left and right
// starting at the critical point until an obstacle is hit, because this prevents unnecessary cells
// behind other obstacles on the same y-value as the critical point
if(critical_point == true)
{
const int ym2 = std::max(0,(int)y-2);
// to the left until a black pixel is hit
for(int dx=-1; x+dx>=0; --dx)
{
uchar& val = cell_map.at<uchar>(y-1,x+dx);
if(val == 255 && cell_map.at<uchar>(ym2,x+dx) == 255)
val = BORDER_PIXEL_VALUE;
else if(val == 0)
break;
}
// to the right until a black pixel is hit
for(int dx=1; x+dx<room_map.cols; ++dx)
{
uchar& val = cell_map.at<uchar>(y-1,x+dx);
if(val == 255 && cell_map.at<uchar>(ym2,x+dx) == 255)
val = BORDER_PIXEL_VALUE;
else if(val == 0)
break;
}
}一旦找到关键点，就从该点开始向左和向右扩展，直到遇到障碍物。在扩展过程中，会在cell_map中标记这些边界。这些标记用于后续的单元格分解。
下图为关键点扩展后的cell_map，也是在经过上述操作后的cell_map
2.3 合并单元格
如上图所示，在经过关键点查找后地图被分割成了多块区域，后续的算法是在每个区域内进行规划，区域之间并不连接。如果区域过多的话，就会发生实际路径效率较差的情况，所以需要将一些覆盖范围较小的区域进行合并，这时候就用到了开篇的参数min_cell_area
cv::Mat cell_map_labels;
const int number_of_cells = mergeCells(cell_map, cell_map_labels, min_cell_area, min_cell_width);mergeCells
int BoustrophedonExplorer::mergeCells(cv::Mat& cell_map, cv::Mat& cell_map_labels, const double min_cell_area, const int min_cell_width)
{
// label all cells
// --> create a label map with 0=walls/obstacles, -1=cell borders, 1,2,3,4...=cell labels
cell_map.convertTo(cell_map_labels, CV_32SC1, 256, 0);
// --> re-assign the cell borders with -1
for (int v=0; v<cell_map_labels.rows; ++v)
for (int u=0; u<cell_map_labels.cols; ++u)
if (cell_map_labels.at<int>(v,u) == BORDER_PIXEL_VALUE*256)
cell_map_labels.at<int>(v,u) = -1;
// --> flood fill cell regions with unique id labels
std::map<int, boost::shared_ptr<BoustrophedonCell> > cell_index_mapping; // maps each cell label --> to the cell object
int label_index = 1;
for (int v=0; v<cell_map_labels.rows; ++v)
{
for (int u=0; u<cell_map_labels.cols; ++u)
{
// if the map has already received a label for that pixel --> skip
if (cell_map_labels.at<int>(v,u)!=65280)
continue;
// fill each cell with a unique id
cv::Rect bounding_box;
const double area = cv::floodFill(cell_map_labels, cv::Point(u,v), label_index, &bounding_box, 0, 0, 4);
cell_index_mapping[label_index] = boost::shared_ptr<BoustrophedonCell>(new BoustrophedonCell(label_index, area, bounding_box));
label_index++;
if (label_index == INT_MAX)
std::cout << "WARN: BoustrophedonExplorer::mergeCells: label_index exceeds range of int." << std::endl;
}
}
std::cout << "INFO: BoustrophedonExplorer::mergeCells: found " << label_index-1 << " cells before merging." << std::endl;
// determine the neighborhood relationships between all cells
for (int v=1; v<cell_map_labels.rows-1; ++v)
{
for (int u=1; u<cell_map_labels.cols-1; ++u)
{
if (cell_map_labels.at<int>(v,u)==-1) // only check the border points for neighborhood relationships
{
const int label_left = cell_map_labels.at<int>(v,u-1);
const int label_right = cell_map_labels.at<int>(v,u+1);
if (label_left>0 && label_right>0)
{
cell_index_mapping[label_left]->neighbors_.insert(cell_index_mapping[label_right]);
cell_index_mapping[label_right]->neighbors_.insert(cell_index_mapping[label_left]);
}
const int label_up = cell_map_labels.at<int>(v-1,u);
const int label_down = cell_map_labels.at<int>(v+1,u);
if (label_up>0 && label_down>0)
{
cell_index_mapping[label_up]->neighbors_.insert(cell_index_mapping[label_down]);
cell_index_mapping[label_down]->neighbors_.insert(cell_index_mapping[label_up]);
}
}
}
}
#ifdef DEBUG_VISUALIZATION
// printCells(cell_index_mapping);
// cv::imshow("cell_map",cell_map);
// cv::waitKey();
#endif
// iteratively merge cells
mergeCellsSelection(cell_map, cell_map_labels, cell_index_mapping, min_cell_area, min_cell_width);
// re-assign area labels to 1,2,3,4,...
int new_cell_label = 1;
for (std::map<int, boost::shared_ptr<BoustrophedonCell> >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc, ++new_cell_label)
for (int v=0; v<cell_map_labels.rows; ++v)
for (int u=0; u<cell_map_labels.cols; ++u)
if (cell_map_labels.at<int>(v,u)==itc->second->label_)
cell_map_labels.at<int>(v,u) = new_cell_label;
std::cout << "INFO: BoustrophedonExplorer::mergeCells: " << cell_index_mapping.size() << " cells remaining after merging." << std::endl;
return cell_index_mapping.size();
}mergeCellsSelection:
void BoustrophedonExplorer::mergeCellsSelection(cv::Mat& cell_map, cv::Mat& cell_map_labels, std::map<int, boost::shared_ptr<BoustrophedonCell> >& cell_index_mapping,
const double min_cell_area, const int min_cell_width)
{
// iteratively merge cells
// merge small cells below min_cell_area with their largest neighboring cell
std::multimap<double, boost::shared_ptr<BoustrophedonCell> > area_to_region_id_mapping; // maps the area of each cell --> to the respective cell
for (std::map<int, boost::shared_ptr<BoustrophedonCell> >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc)
area_to_region_id_mapping.insert(std::pair<double, boost::shared_ptr<BoustrophedonCell> >(itc->second->area_, itc->second));
for (std::multimap<double, boost::shared_ptr<BoustrophedonCell> >::iterator it=area_to_region_id_mapping.begin(); it!=area_to_region_id_mapping.end();)
{
// skip if segment is large enough (area and side length criteria)
if (it->first >= min_cell_area && it->second->bounding_box_.width >= min_cell_width && it->second->bounding_box_.height >= min_cell_width)
{
++it;
continue;
}
// skip segments which have no neighbors
if (it->second->neighbors_.size() == 0)
{
std::cout << "WARN: BoustrophedonExplorer::mergeCells: skipping small cell without neighbors." << std::endl;
++it;
continue;
}
// determine the largest neighboring cell
const BoustrophedonCell& small_cell = *(it->second);
std::multimap<double, boost::shared_ptr<BoustrophedonCell>, std::greater<double> > area_sorted_neighbors;
for (BoustrophedonCell::BoustrophedonCellSetIterator itn = small_cell.neighbors_.begin(); itn != small_cell.neighbors_.end(); ++itn)
area_sorted_neighbors.insert(std::pair<double, boost::shared_ptr<BoustrophedonCell> >((*itn)->area_, *itn));
BoustrophedonCell& large_cell = *(area_sorted_neighbors.begin()->second);
// merge the cells
mergeTwoCells(cell_map, cell_map_labels, small_cell, large_cell, cell_index_mapping);
// update area_to_region_id_mapping
area_to_region_id_mapping.clear();
for (std::map<int, boost::shared_ptr<BoustrophedonCell> >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc)
area_to_region_id_mapping.insert(std::pair<double, boost::shared_ptr<BoustrophedonCell> >(itc->second->area_, itc->second));
it = area_to_region_id_mapping.begin();
#ifdef DEBUG_VISUALIZATION
// printCells(cell_index_mapping);
// cv::imshow("cell_map",cell_map);
// cv::waitKey();
#endif
}
// label remaining border pixels with label of largest neighboring region label
for (int v=1; v<cell_map.rows-1; ++v)
{
for (int u=1; u<cell_map.cols-1; ++u)
{
if (cell_map.at<uchar>(v,u) == BORDER_PIXEL_VALUE)
{
std::set<int> neighbor_labels;
for (int dv=-1; dv<=1; ++dv)
{
for (int du=-1; du<=1; ++du)
{
const int& val = cell_map_labels.at<int>(v+dv,u+du);
if (val>0)
neighbor_labels.insert(val);
}
}
if (neighbor_labels.size() > 0)
{
int new_label = -1;
for (std::multimap<double, boost::shared_ptr<BoustrophedonCell> >::reverse_iterator it=area_to_region_id_mapping.rbegin(); it!=area_to_region_id_mapping.rend(); ++it)
{
if (neighbor_labels.find(it->second->label_) != neighbor_labels.end())
{
cell_map_labels.at<int>(v,u) = it->second->label_;
break;
}
}
}
else
std::cout << "WARN: BoustrophedonExplorer::mergeCells: border pixel has no labeled neighbors." << std::endl;
}
}
}
}mergeTwoCells:
void BoustrophedonExplorer::mergeTwoCells(cv::Mat& cell_map, cv::Mat& cell_map_labels, const BoustrophedonCell& minor_cell, BoustrophedonCell& major_cell,
std::map<int, boost::shared_ptr<BoustrophedonCell> >& cell_index_mapping)
{
// execute merging the minor cell into the major cell
// --> remove border from maps
for (int v=0; v<cell_map.rows; ++v)
for (int u=0; u<cell_map.cols; ++u)
if (cell_map.at<uchar>(v,u) == BORDER_PIXEL_VALUE &&
((cell_map_labels.at<int>(v,u-1)==minor_cell.label_ && cell_map_labels.at<int>(v,u+1)==major_cell.label_) ||
(cell_map_labels.at<int>(v,u-1)==major_cell.label_ && cell_map_labels.at<int>(v,u+1)==minor_cell.label_) ||
(cell_map_labels.at<int>(v-1,u)==minor_cell.label_ && cell_map_labels.at<int>(v+1,u)==major_cell.label_) ||
(cell_map_labels.at<int>(v-1,u)==major_cell.label_ && cell_map_labels.at<int>(v+1,u)==minor_cell.label_)))
{
cell_map.at<uchar>(v,u) = 255;
cell_map_labels.at<int>(v,u) = major_cell.label_;
major_cell.area_ += 1;
}
// --> update old label in cell_map_labels
for (int v=0; v<cell_map_labels.rows; ++v)
for (int u=0; u<cell_map_labels.cols; ++u)
if (cell_map_labels.at<int>(v,u) == minor_cell.label_)
cell_map_labels.at<int>(v,u) = major_cell.label_;
// --> update major_cell
major_cell.area_ += minor_cell.area_;
for (BoustrophedonCell::BoustrophedonCellSetIterator itn = major_cell.neighbors_.begin(); itn != major_cell.neighbors_.end(); ++itn)
if ((*itn)->label_ == minor_cell.label_)
{
major_cell.neighbors_.erase(itn);
break;
}
for (BoustrophedonCell::BoustrophedonCellSetIterator itn = minor_cell.neighbors_.begin(); itn != minor_cell.neighbors_.end(); ++itn)
if ((*itn)->label_ != major_cell.label_)
major_cell.neighbors_.insert(*itn);
// clean all references to minor_cell
cell_index_mapping.erase(minor_cell.label_);
for (std::map<int, boost::shared_ptr<BoustrophedonCell> >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc)
for (BoustrophedonCell::BoustrophedonCellSetIterator itn = itc->second->neighbors_.begin(); itn != itc->second->neighbors_.end(); ++itn)
if ((*itn)->label_ == minor_cell.label_)
{
(*itn)->label_ = major_cell.label_;
break;
}
}
这部分太长了，是真的不想看，反正经过合并后变成了这样，一些细小的房间被处理掉了：
两张放在一起对比看，可以明显的看出部分区域进行了合并，调节min_cell_area可以改变合并的效果
| 合并前 |
合并后 |
2.4 处理单元格
std::vector<std::vector<cv::Point> > cells;
for (int i=1; i<=number_of_cells; ++i)
{
cv::Mat cell_copy(cell_map_labels == i);
std::vector<std::vector<cv::Point> > cellsi;
cv::findContours(cell_copy, cellsi, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
cells.insert(cells.end(), cellsi.begin(), cellsi.end());
}对于每个单元格标签，创建一个副本cell_copy，然后使用cv::findContours函数找到该单元格的轮廓。这些轮廓代表了单元格的边界。
// create generalized Polygons out of the contours to handle the cells
for(size_t cell=0; cell<cells.size(); ++cell)
{
GeneralizedPolygon current_cell(cells[cell], map_resolution);
if(current_cell.getArea()>=min_cell_area)
{
cell_polygons.push_back(current_cell);
polygon_centers.push_back(current_cell.getCenter());
}
else
{
std::cout << "WARN: BoustrophedonExplorer::computeCellDecomposition: dropped cell " << cell+1 << " with area=" << current_cell.getArea() << ". This should only happen for small unconnected cells." << std::endl;
}
}对于每个找到的轮廓，创建一个GeneralizedPolygon对象，该对象基于轮廓和地图分辨率。然后检查每个多边形的面积是否满足最小面积要求。如果满足，将其添加到cell_polygons中；如果不满足，打印警告信息。
Step 3 绘制覆盖路径
// *********************** IV. Determine the cell paths. ***********************
// determine the start cell that contains the start position
std::vector<cv::Point> starting_point_vector(1, starting_position); // opencv syntax
cv::transform(starting_point_vector, starting_point_vector, R);
const cv::Point rotated_starting_point = starting_point_vector[0]; // point that keeps track of the last point after the boustrophedon path in each cell
int start_cell_index = 0;
for(std::vector<GeneralizedPolygon>::iterator cell=cell_polygons.begin(); cell!=cell_polygons.end(); ++cell)
if(cv::pointPolygonTest(cell->getVertices(), rotated_starting_point, false) >= 0)
start_cell_index = cell - cell_polygons.begin();
// determine the visiting order of the cells
std::vector<int> optimal_order;
if (cell_visiting_order == OPTIMAL_TSP)
{
// determine the optimal visiting order of the cells
GeneticTSPSolver tsp_solver;
optimal_order = tsp_solver.solveGeneticTSP(rotated_room_map, polygon_centers, 0.25, 0.0, map_resolution, start_cell_index, 0);
if (optimal_order.size()!=polygon_centers.size())
{
std::cout << "=====================> Genetic TSP failed with 25% resolution, falling back to 100%. <=======================" << std::endl;
optimal_order = tsp_solver.solveGeneticTSP(rotated_room_map, polygon_centers, 1.0, 0.0, map_resolution, start_cell_index, 0);
}
}
else if (cell_visiting_order == LEFT_TO_RIGHT)
{
// we are using an alternative ordering here, which visits the cells in a more obvious fashion to the human observer (though it is not optimal)
std::multimap<int, int> y_coordinate_ordering; // <y-coordinate of room centers, index>
for (size_t i=0; i<polygon_centers.size(); ++i)
y_coordinate_ordering.insert(std::pair<int, int>(polygon_centers[i].y, (int)i));
for (std::multimap<int,int>::iterator it=y_coordinate_ordering.begin(); it!=y_coordinate_ordering.end(); ++it)
optimal_order.push_back(it->second);
}
else
{
std::cout << "Error: BoustrophedonExplorer::getExplorationPath: The specified cell_visiting_order=" << cell_visiting_order << " is invalid." << std::endl;
return;
}
// go trough the cells [in optimal visiting order] and determine the boustrophedon paths
ROS_INFO("Starting to get the paths for each cell, number of cells: %d", (int)cell_polygons.size());
std::cout << "Boustrophedon grid_spacing_as_int=" << grid_spacing_as_int << std::endl;
cv::Point robot_pos = rotated_starting_point; // point that keeps track of the last point after the boustrophedon path in each cell
std::vector<cv::Point2f> fov_middlepoint_path; // this is the trajectory of centers of the robot footprint or the field of view
for(size_t cell=0; cell<cell_polygons.size(); ++cell)
{
computeBoustrophedonPath(rotated_room_map, map_resolution, cell_polygons[optimal_order[cell]], fov_middlepoint_path,
robot_pos, grid_spacing_as_int, half_grid_spacing_as_int, path_eps, max_deviation_from_track, grid_obstacle_offset/map_resolution);
}
// transform the calculated path back to the originally rotated map and create poses with an angle
RoomRotator room_rotation;
std::vector<geometry_msgs::Pose2D> fov_poses; // this is the trajectory of poses of the robot footprint or the field of view, in [pixels]
room_rotation.transformPathBackToOriginalRotation(fov_middlepoint_path, fov_poses, R);
ROS_INFO("Found the cell paths.");
// if the path should be planned for the robot footprint create the path and return here
if (plan_for_footprint == true)
{
for(std::vector<geometry_msgs::Pose2D>::iterator pose=fov_poses.begin(); pose != fov_poses.end(); ++pose)
{
geometry_msgs::Pose2D current_pose;
current_pose.x = (pose->x * map_resolution) + map_origin.x;
current_pose.y = (pose->y * map_resolution) + map_origin.y;
current_pose.theta = pose->theta;
path.push_back(current_pose);
}
return;
}这一部分的作用就是在规划路径，使其能遍历所有的cell，路径生成主要在computeBoustrophedonPath中进行，前置部分通过将各个单元格（cell）的访问顺序建模为一个Tsp问题，并通过遗传算法解决，也就是GeneticTSPSolver tsp_solver部分，最后根据plan_for_footprint参数判断是否将路径转化为实际地图坐标
这一部分我们先不去关心cell的访问顺序，主要看覆盖路径是怎么生成的，也就是computeBoustrophedonPath的实现，这也是覆盖算法的核心
