如下图是更新地图膨胀

void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{//用指针master_array指向主地图，并获取主地图的尺寸，确认seen_数组被正确设置。boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);if (!enabled_ || (cell_inflation_radius_ == 0))return;// make sure the inflation list is empty at the beginning of the cycle (should always be true)ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation");unsigned char* master_array = master_grid.getCharMap();unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();if (seen_ == NULL) {ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL");seen_size_ = size_x * size_y;seen_ = new bool[seen_size_];}else if (seen_size_ != size_x * size_y){ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong");delete[] seen_;seen_size_ = size_x * size_y;seen_ = new bool[seen_size_];}memset(seen_, false, size_x * size_y * sizeof(bool));// We need to include in the inflation cells outside the bounding// box min_i...max_j, by the amount cell_inflation_radius_.  Cells// up to that distance outside the box can still influence the costs// stored in cells inside the box.//边际膨胀min_i -= cell_inflation_radius_;min_j -= cell_inflation_radius_;max_i += cell_inflation_radius_;max_j += cell_inflation_radius_;min_i = std::max(0, min_i);min_j = std::max(0, min_j);max_i = std::min(int(size_x), max_i);max_j = std::min(int(size_y), max_j);// Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle// We use a map<distance, list> to emulate the priority queue used before, with a notable performance boost//接下来遍历bound中的cell，找到cost为LETHAL_OBSTACLE，即障碍物cell，将其以CellData形式放进inflation_cells_[0.0]中，inflation_cells_的定义如下：//std::map<double, std::vector > inflation_cells_;//它是一个映射，由浮点数→CellData数组，CellData这个类定义在inflation_layer.h中，//是专门用来记录当前cell的索引和与它最近的障碍物的索引的。这个映射以距离障碍物的距离为标准给bound内的cell归类，为膨胀做准备；//自然距离为0对应的cell即障碍物cell本身，//目前得到的inflation_cells_只包括障碍物本身。// Start with lethal obstacles: by definition distance is 0.0std::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);unsigned char cost = master_array[index];if (cost == LETHAL_OBSTACLE){obs_bin.push_back(CellData(index, i, j, i, j));}}}// Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they// can overtake previously inserted but farther away cells//通过增加距离处理细胞；新的单元格被附加到相应的距离仓中，因此它们//可以超越先前插入但距离更远的单元格std::map<double, std::vector<CellData> >::iterator bin;for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin){for (int i = 0; i < bin->second.size(); ++i){// process all cells at distance dist_bin.firstconst CellData& cell = bin->second[i];unsigned int index = cell.index_;// ignore if already visitedif (seen_[index]){continue;}seen_[index] = true;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 cellunsigned char cost = costLookup(mx, my, sx, sy);unsigned char old_cost = master_array[index];if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))master_array[index] = cost;elsemaster_array[index] = std::max(old_cost, cost);// attempt to put the neighbors of the current cell onto the inflation listif (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();
}

InflationLayer没有自身的栅格地图要维护，直接在主地图上进行操作，它根据膨胀参数设置用来膨胀的“参考矩阵”，并在主地图上从障碍物出发，不断传播更新，完成对整个地图障碍的膨胀。
不断的从障碍物周围”上下左右“开始膨胀，并且根据参考矩阵去计算不同距离的栅格权重。

 // assign the cost associated with the distance from an obstacle to the cellunsigned char cost = costLookup(mx, my, sx, sy);unsigned char old_cost = master_array[index];if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))master_array[index] = cost;elsemaster_array[index] = std::max(old_cost, cost);

unsigned char cost = costLookup(mx, my, sx, sy);为参考矩阵计算， master_array[index] = cost;为当前cell栅格赋值。

 // attempt to put the neighbors of the current cell onto the inflation listif (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);}

这是考察下一个点，上下左右，如果在障碍物半径内，则继续循环到上述的“ unsigned char cost = costLookup(mx, my, sx, sy);为参考矩阵计算， master_array[index] = cost;为当前cell栅格赋值。”

如此循环即可，类似于A*算法的膨胀。

/**
*@brief给定成本图中某个单元格的索引，将其放入等待障碍物膨胀的列表中
*@param grid成本图
*@param index单元格的索引
*@param mx单元格的x坐标（可以根据索引计算，但可以节省存储时间）
*@param my单元格的y坐标（可以根据索引计算，但可以节省存储时间）
*@param src_x障碍点通货膨胀的x指数开始于
*@param src_y障碍点通货膨胀的y指数始于
*/
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 belowdouble 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 pointif (distance > cell_inflation_radius_)return;// push the cell data onto the inflation list and markinflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));}
}

 double distance = distanceLookup(mx, my, src_x, src_y);

使用计算考察点和障碍物的距离，然后判断是否在膨胀半径内，如果是，则保存到nflation_cells_中用于下一次循环中。

Reference

1. 机器人控制—代价地图Costmap的层概述