amcl的地图表示

amcl
├── include───amcl───map
│                    └── map.h
└── src───amcl───map
                 ├── map.c
                 ├── map_cspace.cpp
                 ├── map_draw.c
                 ├── map_range.c
                 └── map_store.c

地图是进行定位和导航的基本数据对象之一，ROS的各种SLAM或者导航的包都定义了自己的地图结构。比如说，在本系列文章中， 我们可以看到GMapping使用C++模板定义了自己的地图结构， move_base框架还使用了costmap_2d的代价地图。amcl也使用C语言定义了自己的地图结构map_t。 也就是说，在amcl_demo中我们实际上使用了两套地图结构，move_base的代价地图主要是用于导航和避障的，而amcl的map_t则主要是用于定位。

如右边删减的目录树所示，amcl的地图结构由一个头文件map.h和四个c源文件以及一个c++源文件构成。头文件map.h定义了地图结构的各种接口， map.c完成了基本的内存申请释放以及元素访问的函数。map_draw.c用于可视化地图，这里不再赘述。map_range.c中只有一个用于计算给定地图和位姿下扫描距离估计的函数， map_store.c则实现了从占用地图文件中加载地图数据的功能。至于map_cspace.cpp的功能，我们在第二次修改本文的时候再细说。

1. map_t

在头文件map.h中，定义了map_cell_t和map_t两个数据结构， 用于表示删格地图。各个字段看字面意思和注释很好理解，不再赘述。

        typedef struct {
            // 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;

        typedef struct {
            double origin_x, origin_y;
            // Map scale (m/cell)
            double scale; 
            int size_x, size_y;
            map_cell_t *cells;
            double max_occ_dist;
        } map_t;

在分析总体业务逻辑的时候， 我们看到在处理地图消息数据的时候，通过convertMap将接收到的地图消息转换成为这里的map_t对象， 如下面的代码片段所示，首先通过函数map_alloc构建map_t对象。

        map_t* AmclNode::convertMap( const nav_msgs::OccupancyGrid& map_msg ) {
            map_t* map = map_alloc();

然后，根据接收到的地图消息设定对象map的尺寸、原点以及分辨率。

            map->size_x = map_msg.info.width;
            map->size_y = map_msg.info.height;
            map->scale = map_msg.info.resolution;
            map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
            map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;

最后为地图的删格单元申请内存，并填充占用删格。

            map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
            for(int i = 0; i < map->size_x * map->size_y; i++) {
                if(map_msg.data[i] == 0)
                    map->cells[i].occ_state = -1;
                else if(map_msg.data[i] == 100)
                    map->cells[i].occ_state = +1;
                else
                    map->cells[i].occ_state = 0;
            }
            return map;
        }

2. 基本接口

在map.c文件中分别实现了三个函数map_alloc, map_free和map_get_cell，用于创建和销毁一个地图，并获取指定位姿的占用数据。如下面的代码所示， 其中map_alloc只是申请了map_t的内存，并没有为占用删格，即cells字段，申请内存，这里只是赋予了空指针的初值而已。而map_free没有什么好说的，就是释放已经申请的内存而已。

        map_t *map_alloc(void) {
            map_t *map = (map_t*) malloc(sizeof(map_t));
            map->origin_x = 0;
            map->origin_y = 0;
            map->size_x = 0;
            map->size_y = 0;
            map->scale = 0;
            map->cells = (map_cell_t*) NULL;
            return map;
        }
        void map_free(map_t *map) {
            free(map->cells);
            free(map);
            return;
        }

        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;
        }

map_get_cell通过宏MAP_GXWX和MAP_GYWY获取指定位置ox和oy所对应的删格索引，而宏定义MAP_VALID则用于判定删格索引是否超出了地图的边界。 最后通过MAP_INDEX将删格的行列索引转换到占用删格的索引，最终将对应的删格单元地址返回。

下面的代码片段是amcl_node.cpp中AmclNode的成员函数convertMap， 该函数将ROS的占用地图消息转换为map_t的数据结构。在函数的一开始，调用map_alloc函数申请map_t对象的内存。

        map_t* AmclNode::convertMap(const nav_msgs::OccupancyGrid & map_msg) {
            map_t* map = map_alloc();

然后从消息内容中获取地图的尺寸、分辨率、以及原点的信息，并赋予map对象。

            map->size_x = map_msg.info.width;
            map->size_y = map_msg.info.height;
            map->scale = map_msg.info.resolution;
            map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
            map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;

接着为地图删格申请内存，并根据消息中的占用数据初始化各个地图删格的占用状态。其中-1表示空闲区域，0为未知区域，+1代表被占用的删格。

            map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y); 
            for(int i = 0; i < map->size_x * map->size_y; i++) {
                if(map_msg.data[i] == 0)
                    map->cells[i].occ_state = -1;
                else if(map_msg.data[i] == 100)
                    map->cells[i].occ_state = +1;
                else
                    map->cells[i].occ_state = 0;
            }

最后将新构建的删格地图对象返回。

            return map;
        }

3. 预测Range

amcl实现了《Probabilistic Robotics》中所描述的beam_range_finder_model， 该模型需要计算无噪声的情况下传感器的测量值，文件map_range.c中所实现的函数map_calc_range就是用来估计这一测量值的。根据源码的注释，可以了解到它使用的是Bresenham raytracing的算法。

如下面的代码片段所示，map_calc_range有5个输入参数，其中map是我们的地图对象，ox、oy、oa分别指示了机器人的位置坐标和方向角，max_range则是激光传感器的量程。

        double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range) {

首先通过宏MAP_GXWX和MAP_GYWY获取扫描波束的起点和终点所对应的删格索引。

            int x0 = MAP_GXWX(map,ox);
            int y0 = MAP_GYWY(map,oy);
            int x1 = MAP_GXWX(map,ox + max_range * cos(oa));
            int y1 = MAP_GYWY(map,oy + max_range * sin(oa));

当扫描波束的y轴差异大于x轴差异时，将该方向标记为陡峭的(steep = 1)。按:其实不是很理解为什么要这么做，我们直接沿着这一波束的方向逐一检查各个删格就可以了呀。

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

如果是陡峭的，则交换x和y的坐标。

            if (steep) {
                tmp = x0; x0 = y0; y0 = tmp;
                tmp = x1; x1 = y1; y1 = tmp;
            }

下面分别计算x和y轴的索引扫描偏差量，并为error和deltaerr赋予初值。

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

然后根据x和y轴的发展方向，设定搜索方向。

            int xstep = (x0 < x1) ? 1 : -1;
            int ystep = (y0 < y1) ? 1 : -1;

我们先检查机器人的位置坐标，确保它在地图的上，并且没有处于未知区域或者占用删格中。

            int x = x0;
            int y = y0;
            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;
        }

4. 完

本文分析了amcl中用于描述地图的数据结构，介绍了在map.c文件中实现的创建和销毁地图对象以及访问地图元素的方法，并细述了根据地图数据和机器人位姿估计无噪声情况下的传感器测量值的方法。