2D SLAM_Gmapping（3）_從Gmapping建圖到Amcl定位中地圖的變換過程

從Gmapping建圖到Amcl定位中地圖的變換過程可以分爲四個步驟：
第一步：Gmapping生成的概率地圖=>pgm圖片以及對應的yaml文件(map_saver)
第二步：pgm圖片以及對應的yaml文件=>ros的OccpancyGrid數據格式發佈（map_server)
第三步：ros的OccpancyGrid數據格式=>amcl需要的佔據柵格地圖（amcl的convertMap())
接下來看看相對應的程序：

第一步：Gmapping生成的概率地圖=>pgm圖片以及對應的yaml文件(map_saver)

通過map_saver將gmapping建圖後的數據保存爲一張.pgm圖片及對應的yaml文件。

for(unsigned int y = 0; y < map->info.height; y++) 
      {
        for(unsigned int x = 0; x < map->info.width; x++) 
        {
          unsigned int i = x + (map->info.height - y - 1) * map->info.width;
          if (map->data[i] >= 0 && map->data[i] <= threshold_free_) 
          { // [0,free)
            fputc(254, out);
          } 
          else if (map->data[i] >= threshold_occupied_) 
          { // (occ,255]
            fputc(000, out);
          } 
          else 
          { //occ [0.25,0.65]
            fputc(205, out);
          }
        }
      }
      fclose(out);
      std::string mapmetadatafile = mapname_ + ".yaml";
      ROS_INFO("Writing map occupancy data to %s", mapmetadatafile.c_str());
      FILE* yaml = fopen(mapmetadatafile.c_str(), "w");

從這裏基本上可以看出通過gmapping生成的地圖數據是通過OccpancyGrid的形式發佈過來的，在進入map_salver之前就已經做過了轉換，首先看佔據閾值，拿過來作比較的是65和25兩個值。

  int threshold_occupied = 65;
  int threshold_free = 25;

然後把map->data[i]與threshold_occupied或threshold_free比較，一般來說，由gmapping生成的概率爲(0-1)的數，因此這裏的map-data[i]是經gmapping內部已作處理，這裏暫不做討論。繼續往下看，這裏比較完之後，如果滿足條件，相應的寫入

    fputc(254, out);
    fputc(000, out);
    fputc(205, out);

很明顯，通過gmapping存儲的每個柵格的概率存儲在了
i=x+(map->info.height-y-1)*map->info.width;
map->data[i]中存儲的便是(x,y)對應的佔用概率。

第二步：pgm圖片以及對應的yaml文件=>ros的OccpancyGrid數據格式發佈（map_server)

map_server實現將地圖圖像數據中的顏色值轉換爲三元佔有值：free（0），occupated（100）和unknown（-1）。

  // Copy pixel data into the map structure
  pixels = (unsigned char*)(img->pixels);
  for(j = 0; j < resp->map.info.height; j++)
  {
    for (i = 0; i < resp->map.info.width; i++)
    {
      // Compute mean of RGB for this pixel
      p = pixels + j*rowstride + i*n_channels;
      color_sum = 0;
      for(k=0;k<avg_channels;k++)
        color_sum += *(p + (k));
      color_avg = color_sum / (double)avg_channels;

      if (n_channels == 1)
          alpha = 1;
      else
          alpha = *(p+n_channels-1);
      if(negate)
        color_avg = 255 - color_avg;

      if(mode==RAW)
      {
          value = color_avg;
          resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
          continue;
      }
      // If negate is true, we consider blacker pixels free, and whiter
      // pixels occupied.  Otherwise, it's vice versa.
      occ = (255 - color_avg) / 255.0;

      // Apply thresholds to RGB means to determine occupancy values for
      // map.  Note that we invert the graphics-ordering of the pixels to
      // produce a map with cell (0,0) in the lower-left corner.
      if(occ > occ_th)
        value = +100;
      else if(occ < free_th)
        value = 0;
      else if(mode==TRINARY || alpha < 1.0)
        value = -1;
      else 
      {
        double ratio = (occ - free_th) / (occ_th - free_th);
        value = 99 * ratio;
      }

      resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
    }
  }

當與閾值參數比較時，圖像像素的佔用概率計算如下：occ =（255-color_avg）/ 255.0其中color_avg是通過在所有通道上求平均值而得到的8位值，例如，如果圖像是24位顏色，具有顏色0x0a0a0a的像素具有0.96的概率，這是非常佔用的。顏色0xeeeeee產生0.07，這是非常空的。

第三步：ros的OccpancyGrid數據格式=>amcl需要的佔據柵格地圖（amcl的convertMap())

通過handleMapMessage()函數接收到msg.info.width×msg.info.height的0.025m/pix的地圖。然後經過convertMap()將通過map（nav_msgs / OccupancyGrid）話題接收地圖，轉換爲map_t結構體。具體如下：

  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;
  // Convert to player format
  map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
  ROS_ASSERT(map->cells);
  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;//未知
    }
  }

下表粗略地展示地圖值的變換：

	圖片像素值	OccpancyGrid msg	佔據柵格
佔用	occ>occ_thresh	100	+1
空閒	occ<fress_thresh	0	-1
未知	mode==TRINARY ,alpha < 1.0/其它	-1/99 * ratio	0