膨胀地图（一）
前面看过了静态地图与障碍物地图，但是对于路径规划而言，这两个地图是不够的，还需要第三张地图，也就是膨胀地图。
膨胀地图的意思还是比较好理解的，就是将地图的障碍物进行膨胀。为什么要这么操作呢，主要是考虑路径规划时，对于静态地图而言，不太好进行路径规划，因为一个栅格点虽然它可能是空闲的，但是如果它离障碍物很近的话，实际上这个点机器人是不能到达的，所以我们需要对地图按照机器人半径进行膨胀，这样子剩下的点才是真正可以运动的区域。
在move_base中，膨胀地图的内容不算多，只有简单的几个函数，这里简单看一下。
1、computeCaches
这个函数是一个预处理函数，所以我们将其放在最前面来看。这个函数主要执行的功能是预计算障碍物周围点到障碍物的距离以及代价值关系，后续会根据查表法来获取具体数据：
void InflationLayer::computeCaches() { //Case 1:如果栅格的膨胀半径是0，那就直接返回，啥也不用干了。 if (cell_inflation_radius_ == 0) return; // based on the inflation radius... compute distance and cost caches //Case 2:如果栅格的膨胀半径不等于缓存的栅格膨胀半径，那么: 基于膨胀半径...计算距离和成本缓存 if (cell_inflation_radius_ != cached_cell_inflation_radius_) { deleteKernels(); cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2]; //重新分配内存，建立cached_costs_ cached_distances_ = new double*[cell_inflation_radius_ + 2]; //重新分配内存，建立cached_distances_ //建立两层循环，从0到栅格膨胀半径+1，逐步填充代价和距离 for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) { cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2]; cached_distances_[i] = new double[cell_inflation_radius_ + 2]; for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) {//hypot函数是一个C++标准库函数，它计算两个浮点数的平方和的平方根，即计算直角三角形的斜边长度。 cached_distances_[i][j] = hypot(i, j); } } //更新缓存的膨胀半径 cached_cell_inflation_radius_ = cell_inflation_radius_; } //计算代价 for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) { for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) { cached_costs_[i][j] = computeCost(cached_distances_[i][j]); } } }
这里的cell_inflation_radius_就是机器人的半径，当机器人的半径不为0时，需要计算一个膨胀表cached_costs_，它的作用类似于下图：
computeCost这个函数本身是计算一个点的代价值的，它的具体实现如下：
/** @brief Given a distance, compute a cost. * @param distance The distance from an obstacle in cells * @return A cost value for the distance */ //首先进行距离的判断，如果距离为0，则返回代价为致命障碍物的代价值LETHAL_OBSTACLE; //再继续往下判断，距离分辨率是否小于内切半径，是的话，则返回代价为机器人内切圆膨胀障碍物的代价值INSCRIBED_INFLATED_OBSTACLE; //否的话，则代价按照欧几里德距离递减，欧几里德距离为距离分辨率，定义factor因子为指数函数，最后计算出cost值； virtual inline unsigned char computeCost(double distance) const { unsigned char cost = 0; if (distance == 0) cost = LETHAL_OBSTACLE; else if (distance * resolution_ <= inscribed_radius_) cost = INSCRIBED_INFLATED_OBSTACLE; else { // make sure cost falls off by Euclidean distance double euclidean_distance = distance * resolution_; double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_)); cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor); } return cost; }
这段代码与上面的图片是契合的，对于障碍物的点，其值会为254，而离障碍物距离小于机器人半径的区域则是赋值253，再向外的点的值则是指数型下降。
2、updateCosts
这个函数是膨胀地图中的主要处理函数了，地图从障碍物地图变成膨胀地图就是通过这个函数实现的。首先需要获取一张现有的地图以及地图的尺寸信息：
unsigned char* master_array = master_grid.getCharMap();//获取地图 unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();//获取网格x,y坐标
在获取到地图后，算法会遍历这张地图，然后将所有的障碍物点都先记录下来：
std::vector<CellData>& obs_bin = inflation_cells_[0.0]; for (int j = min_j; j < max_j; j++) { for (int i = min_i; i < max_i; i++) { int index = master_grid.getIndex(i, j); //1)从master_grid地图获取(i,j)对应的索引index unsigned char cost = master_array[index]; //2)从master_grid地图获取索引对应的代价cost if (cost == LETHAL_OBSTACLE) //3)先判断代价是否等于致命障碍物对应的代价，如果是，压入 { //注意这里的obs_bin是一个指针，数据其实是存储在inflation_cells_下 //std::map<double, std::vector<CellData> > inflation_cells_; obs_bin.push_back(CellData(index, i, j, i, j)); } } }
注意这里记录的点都被记录在inflation_cells_中了，这是一个std::map<double, std::vector >类型的数据。
既然我们知道了所有的障碍物点了，下一步自然是需要对其进行扩张了，这里的扩张使用的是两个for循环实现的：
//根据膨胀队列，进行膨胀 //通过上面的操作，现在inflation_queue_里面全部都是obstacle cell，这些cell被打包成CellData类型，包含了这些cell本身的索引，最近障碍物的索引（即它们本身），距离（0） std::map<double, std::vector<CellData> >::iterator bin; for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin) { //遍历每一个障碍物点 //map键值对映射，键是距离最近的障碍物的距离，值是cell //second是“键”“值”对中的“值”，即遍历次数为celldata的个数 for (int i = 0; i < bin->second.size(); ++i) { // process all cells at distance dist_bin.first //在该“double键”下记录迭代到的celldata const CellData& cell = bin->second[i]; //记录该celldata的索引index unsigned int index = cell.index_; // ignore if already visited 如果已访问过，则忽略 if (seen_[index]) { continue; } seen_[index] = true; //得到该cell的坐标和离它最近的障碍物的坐标 unsigned int mx = cell.x_; unsigned int my = cell.y_; unsigned int sx = cell.src_x_; unsigned int sy = cell.src_y_; // assign the cost associated with the distance from an obstacle to the cell //4)更新膨胀队列中点的cost 值 //根据该CELL与障碍的距离来分配cost unsigned char cost = costLookup(mx, my, sx, sy); unsigned char old_cost = master_array[index]; //从master_grid查找指定索引index的代价 //如果old_cost无信息或者cost≥内接膨胀障碍物，将cost更新到master_array if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE))) master_array[index] = cost; else//否则比较新旧大小，将大的cost更新到master_array master_array[index] = std::max(old_cost, cost); // attempt to put the neighbors of the current cell onto the inflation list //上面首先从inflation_cells_中取出最大distance的cell，再将这个cell的前后左右四个cell都塞进inflation_cells_。 //由于下面关于enqueue的调用，最后两个参数（sx, sy)没有改变，所以保证了这个索引一定是obstacle cell。 //由于在 enqueue入口会检查 if (!seen_[index])，这保证了这些cell不会被重复的塞进去。 //由于这是一个priority queue，因此障碍物的周边点，每次都是离障碍物最远的点被弹出，并被检查，这样保证了这种扩张是朝着离障碍物远的方向进行。 //尝试将当前单元的邻居(前后左右)放到队列中 if (mx > 0) enqueue(index - 1, mx - 1, my, sx, sy); if (my > 0) enqueue(index - size_x, mx, my - 1, sx, sy); if (mx < size_x - 1) enqueue(index + 1, mx + 1, my, sx, sy); if (my < size_y - 1) enqueue(index + size_x, mx, my + 1, sx, sy); } } inflation_cells_.clear();
其实上述这里实现的是一个对可能点的遍历，但是比较有意思的是一般这种遍历采用的都是while居多，但是这里使用的是一个对于map容器的for循环。
它的主体思想是这样子的：
1.将所有障碍物点都存储到map容器内
2.遍历map容器，取出其中每一个障碍物点，遍历其周围四邻近点，计算四邻近点到最近障碍物的距离以及其代价值，并存储到map容器内
3.遍历这些离障碍物较近的点，遍历其周围四邻近点，找到下一组比当前点更远离障碍物的点，计算其代价值并存储
4.循环步骤3直到剩下的点离障碍物距离都超出阈值，跳出for循环。
注意这里for循环为什么能够实现这个功能，跟上面的数据类型有很大关系：std::map<double, std::vector >的数据类型中第一位数据为点到障碍物的距离，初始时，map下存储的点都是障碍物的点，所以其键值都为0，然后遍历这些点的过程中会产生一系列四邻近点，这些点到障碍物的距离都为1个栅格的值，因此map下会产生一个新的键值以及一组新的地图点信息，这样子就实现了inflation_cells_的扩张以及inflation_cells_的循环遍历。而当所有接近障碍物的点都遍历完成后，将不会产生新的键值，则inflation_cells_的遍历也就结束了。
另外这里面还有一个比较重要的东西是：
// ignore if already visited 如果已访问过，则忽略 if (seen_[index]) { continue; } seen_[index] = true;
对于每一个点都只遍历一次，这样做的方式是为了防止重复遍历导致的数据异常，因为对于上述的遍历过程而言，前一时刻遍历过的点其离障碍物的距离值一定是比下一个循环遍历时离障碍物的距离值更近的（以障碍物为起点向外扩张）。
3、enqueue
这是膨胀地图中另外一个主要的函数，它的作用是扩散，即从障碍物点向外逐渐扩散直到所有离障碍物较近的点都被遍历完。
/** * @brief Given an index of a cell in the costmap, place it into a list pending for obstacle inflation * @param grid The costmap * @param index The index of the cell * @param mx The x coordinate of the cell (can be computed from the index, but saves time to store it) * @param my The y coordinate of the cell (can be computed from the index, but saves time to store it) * @param src_x The x index of the obstacle point inflation started at * @param src_y The y index of the obstacle point inflation started at */ inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my, unsigned int src_x, unsigned int src_y) { if (!seen_[index]) { // we compute our distance table one cell further than the inflation radius dictates so we can make the check below //找它和最近的障碍物的距离，如果超过了阈值，则直接返回 //如果不超过阈值，就把它也加入inflation_cells_数组里 double distance = distanceLookup(mx, my, src_x, src_y); // we only want to put the cell in the list if it is within the inflation radius of the obstacle point if (distance > cell_inflation_radius_) return; // push the cell data onto the inflation list and mark inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y)); } }
在updateCosts中，对于每一个点，会调用enqueue函数遍历其周围的四个点，在enqueue函数中，这个点如果已经被遍历过一次则跳过，否则会根据这个点离最近的障碍物的距离是否小于cell_inflation_radius_来决定是否需要对其进行updateCosts的计算，对于需要的点会被push进inflation_cells_，也就是我们第二个函数中两层for循环执行的点。
注意这里的cell_inflation_radius_与第一部分中计算时使用的inscribed_radius_是两个参数，这个参数应该会比inscribed_radius_要大一点。如果两个设置成一样的话应该是类似第一部分图中只保留254与253参数的格子，消除所有指数下降部分。
至此膨胀地图基本就看完了，这个部分在没看之前感觉比较高大上，但是实际看完发现东西其实不多。总结来说，就是预先计算一个查表，表中记录一组到达障碍物不同距离的点的代价值。然后遍历每一个障碍物点，这些障碍物点同步向外扩散，求其周围点到障碍物的距离，根据查表法求周围栅格的代价值。直到所有点都扩散到离障碍物较远的距离后，一张膨胀地图就算是计算完成了。
