(笔记)（3）AMCL包源码分析 | 输入与map文件夹 - tdyizhen1314

(笔记)（3）AMCL包源码分析 | 输入与map文件夹

AMCL包从外界接收进来的就只有一张2D栅格地图（物理形式为：一张图片）。这张图片可以是PGM格式，也可以是JPG格式。不管是什么格式，先进入AMCL包的map文件夹里的相关函数处理，解析出这张图片上所包含的信息。

图1. AMCL包map文件夹展开

1.栅格地图定义

如图2所示，直观地来看这张作为AMCL包输入的图片，灰色的地方是未知区域，白色的地方是可通行区域，黑色的边框是不可通行区域。从栅格地图的角度来描述，一个栅格单元的占据状态描述，-1表示空闲，0表示未知，+1表示占据。

图2. 一张2D栅格地图

在map.h中定义一个map_t结构体来表示一张栅格地图的数据结构。其中定义了地图的原点，尺寸，基本单元，跟似然域有关的最大占据距离，代码如下：

// Description for a map 描述一张栅格地图
typedef struct
{
 //地图原点
 // Map origin; the map is a viewport onto a conceptual larger map.
  double origin_x, origin_y;
  
  //地图分辨率
  // Map scale (m/cell)
  double scale;
  
  //地图尺寸：长宽
  // Map dimensions (number of cells)
  int size_x, size_y;
 
  //地图基本单元，视为一个栅格
  // The map data, stored as a grid
  map_cell_t *cells;

  //跟似然域有关的最大占据距离
  // Max distance at which we care about obstacles, for constructing
  // likelihood field
  double max_occ_dist;
  
} map_t;

关于栅格地图的基本单元-栅格单元，也定义一个map_cell_t结构体来表示。其中定义里栅格单元的占据状态occ_state，以及该栅格单元到最近呈现被占据状态的栅格单元的距离occ_dist。

// Description for a single map cell.描述单个栅格单元
typedef struct
{
  //该栅格单元的占据状态，-1表示空闲，0表示未知，+1表示占据
  // Occupancy state (-1 = free, 0 = unknown, +1 = occ)
  int occ_state;

  //该栅格单元到最近呈现被占据状态的栅格单元的距离
  // Distance to the nearest occupied cell
  double occ_dist;

  // Wifi levels
  //int wifi_levels[MAP_WIFI_MAX_LEVELS];

} map_cell_t;

2.利用坐标系转换查找栅格单元

那怎么能将一张图片映射到栅格地图，甚至细致到能读取栅格地图中每个栅格单元的占据状态呢？如图3所示为栅格地图坐标系下栅格单元的序号index与栅格单元的坐标(i,j)对应关系。

图3. 栅格地图坐标系下栅格单元的序号index与栅格单元的坐标(i,j)对应关系

要是可以的话，能从世界坐标系下的坐标(x,y)转换到map坐标系下的坐标(i,j),再将map坐标系下的坐标(i,j)转换成栅格单元的序号，那可真是太好了，那样就能直捣黄龙地找到我们想要的栅格单元，对它进行小小的改造。

Dicho y hecho !(说做就做！)

首先，将世界坐标(世界坐标系下的x,y)转换成地图坐标(map坐标系下的i,j)：

公式部分：

代码部分：

// Convert from world coords to map coords
#define MAP_GXWX(map, x) (floor((x - map->origin_x) / map->scale + 0.5) + map->size_x / 2)
#define MAP_GYWY(map, y) (floor((y - map->origin_y) / map->scale + 0.5) + map->size_y / 2)

其次，在给定地图坐标(map坐标系下的i,j)转换成栅格单元的序号index:

公式部分：

代码部分：

// Compute the cell index for the given map coords.
#define MAP_INDEX(map, i, j) ((i) + (j) * map->size_x)

那么，怎么将地图坐标(map上的i,j)转换成世界坐标(世界坐标系下的x,y)呢？

公式部分：

代码部分：

// Convert from map coords to world coords
#define MAP_WXGX(map, i) (map->origin_x + ((i) - map->size_x / 2) * map->scale)
#define MAP_WYGY(map, j) (map->origin_y + ((j) - map->size_y / 2) * map->scale)

在map_store.cpp中实现了读取一张图片到解析出栅格单元的一系列具体操作，如下面的代码所示：

// Load an occupancy grid 加载占据栅格，输入为map_t格式，文件，地图的分辨率等
int map_load_occ(map_t *map, const char *filename, double scale, int negate)
{
  FILE *file;
  char magic[3];
  int i, j;
  int ch, occ;
  int width, height, depth;
  map_cell_t *cell;

  // Open file
  file = fopen(filename, "r");
  if (file == NULL)
  {
    fprintf(stderr, "%s: %s\n", strerror(errno), filename);
    return -1;
  }

  // Read ppm header
  
  if ((fscanf(file, "%2s \n", magic) != 1) || (strcmp(magic, "P5") != 0))
  {  
    //PGM格式的图片，其实就是已经存在的栅格地图
    fprintf(stderr, "incorrect image format; must be PGM/binary");
    fclose(file);
    return -1;
  }

  // Ignore comments
  while ((ch = fgetc(file)) == '#')
    while (fgetc(file) != '\n');
  ungetc(ch, file);

  // Read image dimensions
  if(fscanf(file, " %d %d \n %d \n", &width, &height, &depth) != 3)
  {
    fprintf(stderr, "Failed ot read image dimensions");
    return -1;
  }

  // Allocate space in the map
  if (map->cells == NULL)
  {
    map->scale = scale;
    map->size_x = width;
    map->size_y = height;
    //根据传入地图的长宽，创建map_t格式下map的size大小
    map->cells = calloc(width * height, sizeof(map->cells[0]));
  }
  else
  {
    if (width != map->size_x || height != map->size_y)
    {
      //PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions");
      return -1;
    }
  }

  // Read in the image 读取这张图片
  for (j = height - 1; j >= 0; j--)
  {
    for (i = 0; i < width; i++)
    {
      /* 关于fgetc()函数
       在文件内部有一个位置指针，用来指向当前读写到的位置，也就是读写到第几个字节。
      在文件打开时，该指针总是指向文件的第一个字节。使用 fgetc() 函数后，
      该指针会向后移动一个字节，所以可以连续多次使用 fgetc() 读取多个字符。
      */
      //从文件里读取一个字符，并保存到变量ch中
      ch = fgetc(file);

      // Black-on-white images
      if (!negate)
      {
        if (ch < depth / 4)
          occ = +1;
        else if (ch > 3 * depth / 4)
          occ = -1;
        else
          occ = 0;
      }

      // White-on-black images
      else
      {
        if (ch < depth / 4)
          occ = -1;
        else if (ch > 3 * depth / 4)
          occ = +1;
        else
          occ = 0;
      }

      if (!MAP_VALID(map, i, j))
        continue;
      //栅格单元
      cell = map->cells + MAP_INDEX(map, i, j);
      //栅格单元的状态填充
      cell->occ_state = occ;
    }
  }
  
  fclose(file);
  
  return 0;
}

在map.cpp中实现了获取指定点（世界坐标系下的坐标x,y,a）的栅格单元cell的操作，如下代码所示。

//获取指定点的栅格  
// Get the cell at the given point
map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa)
{
  int i, j;
  map_cell_t *cell;

  i = MAP_GXWX(map, ox);
  j = MAP_GYWY(map, oy);
  
  if (!MAP_VALID(map, i, j))
    return NULL;

  cell = map->cells + MAP_INDEX(map, i, j);
  return cell;
}

那么完成了从一张图片到栅格单元的一系列操作，还要干点什么呢？

3.栅格单元到最近occupied栅格单元的距离计算：Bresenham画线法

我们接着来聊聊栅格单元到最近呈现被占据状态的栅格单元的距离occ_dist吧。毕竟这才是我们拿到地图，处理地图信息之后的目的。很显然，计算机不会从一张地图上直接读取到这个occ_dist，肯定得想个办法才行。这时候我们记起了Bresenham画线法。

图4.Bresenham画线法

在AMCL包map文件夹下的map_range.cpp具体实现了这个过程。

//从地图里提取单个距离
// Extract a single range reading from the map.  Unknown cells and/or
// out-of-bound cells are treated as occupied, which makes it easy to
// use Stage bitmap files.
//传入地图，世界坐标系下坐标(x,y,a)
double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range)
{
  // Bresenham raytracing
  int x0,x1,y0,y1;
  int x,y;
  int xstep, ystep;
  char steep;
  int tmp;
  int deltax, deltay, error, deltaerr;

  x0 = MAP_GXWX(map,ox);//世界坐标系下的转换成map坐标系下的i,j
  y0 = MAP_GYWY(map,oy);
  
  x1 = MAP_GXWX(map,ox + max_range * cos(oa));//世界坐标系下的转换成map坐标系下的i,j
  y1 = MAP_GYWY(map,oy + max_range * sin(oa));

  if(abs(y1-y0) > abs(x1-x0))
    steep = 1;
  else
    steep = 0;

  if(steep)
  {
    tmp = x0;
    x0 = y0;
    y0 = tmp;

    tmp = x1;
    x1 = y1;
    y1 = tmp;
  }

  deltax = abs(x1-x0);
  deltay = abs(y1-y0);
  error = 0;
  deltaerr = deltay;

  x = x0;
  y = y0;

  if(x0 < x1)
    xstep = 1;
  else
    xstep = -1;
  if(y0 < y1)
    ystep = 1;
  else
    ystep = -1;

  if(steep)
  {
    if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
      return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
  }
  else
  {
    if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
      return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
  }

  while(x != (x1 + xstep * 1))
  {
    x += xstep;
    error += deltaerr;
    if(2*error >= deltax)
    {
      y += ystep;
      error -= deltax;
    }

    if(steep)
    {
      if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
        return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
    }
    else
    {
      if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
        return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
    }
  }
  return max_range;
}

通过上面的操作，每个栅格单元cell喜气洋洋手握一个距离最近被占据栅格occupied_cell的距离occ_dist。我们要对这些栅格单元们cells进行管理一下，这就进入了map_cspace.cpp。上面提到，在定义地图结构体时，有定义一个max_occ_dist,这个跟似然域有关，似然域跟什么有关呢？跟传感器的观测模型有关系。

4.使用优先队列对栅格单元进行管理

  //map结构体截取部分:max_occ_dist
  //跟似然域有关的最大占据距离
  // Max distance at which we care about obstacles, for constructing
  // likelihood field
  double max_occ_dist;

现在直接贴map_cspace.cpp的代码，在代码块里加入一些说明应该能理解得差不多了。

 //定义一个CellData类
  class CellData
  {
  public:
    map_t* map_;
    unsigned int i_, j_;
    unsigned int src_i_, src_j_;
  };
  //定义一个CachedDistanceMap类
  class CachedDistanceMap
  {
  public:
    CachedDistanceMap(double scale, double max_dist) : 
      distances_(NULL), scale_(scale), max_dist_(max_dist) //构造函数，初始化列表
    {
      cell_radius_ = max_dist / scale;
      distances_ = new double *[cell_radius_+2];
      for(int i=0; i<=cell_radius_+1; i++)
      {
	      distances_[i] = new double[cell_radius_+2];
        for(int j=0; j<=cell_radius_+1; j++)
	      {
	        distances_[i][j] = sqrt(i*i + j*j);
	      }
      }
    }
    ~CachedDistanceMap()//析构函数
    {
      if(distances_)
      {
	      for(int i=0; i<=cell_radius_+1; i++)
	         delete[] distances_[i];
	      delete[] distances_;
      }
    }
    double** distances_;//二维指针数组
    double scale_;
    double max_dist_;
    int cell_radius_;
};

// 重载运算符 <；用于比较两个栅格单元
bool operator<(const CellData& a, const CellData& b)
{
  return a.map_->cells[MAP_INDEX(a.map_, a.i_, a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist;
}

CachedDistanceMap* get_distance_map(double scale, double max_dist)
{
  static CachedDistanceMap* cdm = NULL;

  if(!cdm || (cdm->scale_ != scale) || (cdm->max_dist_ != max_dist))
  {
    if(cdm)
      delete cdm;
    cdm = new CachedDistanceMap(scale, max_dist);//重新在自由存储区分配一块内存
  }

  return cdm;
}
//使用优先队列pripority_queue 对栅格单元进行管理
void enqueue(map_t* map, int i, int j,
	     int src_i, int src_j,
	     std::priority_queue<CellData>& Q,
	     CachedDistanceMap* cdm,
	     unsigned char* marked)
{
  if(marked[MAP_INDEX(map, i, j)])
    return;

  int di = abs(i - src_i);
  int dj = abs(j - src_j);
  double distance = cdm->distances_[di][dj];

  if(distance > cdm->cell_radius_)
    return;
 //如何填充map里的栅格单元的occ_dist?如下：
  map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;

  CellData cell;
  cell.map_ = map;
  cell.i_ = i;
  cell.j_ = j;
  cell.src_i_ = src_i;
  cell.src_j_ = src_j;

  Q.push(cell);

  marked[MAP_INDEX(map, i, j)] = 1;
}

//更新cspace(configuration space)的距离值
// Update the cspace distance values 
void map_update_cspace(map_t *map, double max_occ_dist)
{
  unsigned char* marked;
  std::priority_queue<CellData> Q;//定义一个优先队列priority_queue

  marked = new unsigned char[map->size_x*map->size_y];//在自由存储区里重新分配内存
  memset(marked, 0, sizeof(unsigned char) * map->size_x*map->size_y);
  
  //跟似然域有关的地图的最大占据距离
  map->max_occ_dist = max_occ_dist;

  CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist);

  // Enqueue all the obstacle cells 排列所有occupied 栅格
  CellData cell;
  cell.map_ = map;
  for(int i=0; i<map->size_x; i++)
  {
    cell.src_i_ = cell.i_ = i;
    //根据栅格单元的占据状态进行判断，从而对栅格单元的occ_dist进行更新
    for(int j=0; j<map->size_y; j++)
    {
      if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1)
      {
	      map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
	      cell.src_j_ = cell.j_ = j;
	      marked[MAP_INDEX(map, i, j)] = 1;
	      Q.push(cell);
      }
      else
	      map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
    }
  }

  while(!Q.empty())
  {
    CellData current_cell = Q.top();
    if(current_cell.i_ > 0)
      enqueue(map, current_cell.i_-1, current_cell.j_, 
	      current_cell.src_i_, current_cell.src_j_,
	      Q, cdm, marked);
    if(current_cell.j_ > 0)
      enqueue(map, current_cell.i_, current_cell.j_-1, 
	      current_cell.src_i_, current_cell.src_j_,
	      Q, cdm, marked);
    if((int)current_cell.i_ < map->size_x - 1)
      enqueue(map, current_cell.i_+1, current_cell.j_, 
	      current_cell.src_i_, current_cell.src_j_,
	      Q, cdm, marked);
    if((int)current_cell.j_ < map->size_y - 1)
      enqueue(map, current_cell.i_, current_cell.j_+1, 
	      current_cell.src_i_, current_cell.src_j_,
	      Q, cdm, marked);

    Q.pop();
  }

  delete[] marked;
}

转自：3.AMCL包源码分析 | 输入与map文件夹 - 知乎 (zhihu.com)

posted on 2022-09-14 13:51 tdyizhen1314 阅读(275) 评论(0) 编辑收藏举报

会员力量，点亮园子希望

刷新页面返回顶部