cartographer_ros的submap获取与保存

cartographer:occupancy_grid_node_main

本文分析cartographer 使用二维激光传感器的SLAM, google cartographer的地图生成在occupancy_grid_node_main中实现。先放上生成的地图，走廊约为90m*60m 。左边的range_data_inserter中的insert_free_space = true，右边false。表示是否更新raytracy中的free栅格的概率。

系统简介
1. 系统采用local和global的方法来优化位姿 ξ=(ξx,ξy,ξz) ; 在local的方法中，每一帧与submap匹配；使用的是非线性优化将激光和地图对齐。因为在扫描匹配的过程中会累计误差，因此需要全局的方法来回环消除误差。
2. 在local的匹配中，采用的是类似hector_slam的线性插值的方式，不过为了解决双线性插值的不连续性，采用了双三线性插值； 在global的方法中借鉴了olson的相关性扫描匹配方式，通过全局位姿优化来解决误差累计。
3. 在子图构建完成(已经加入足够的激光数据帧)， 会用来做loop closure，当激光现在的位姿估计和之前的子图足够近的时候(再次访问一个地图)，在搜索窗口内，完成类似与olson的相关性扫描匹配，如果匹配结构很好就增加loop closing constraint 。

子图介绍

首先分析的是输出的地图，由次来分析采用的算法结构。这个ROS节点的任务是接受cartographer_node节点运行计算得到的子图序列和子图内容，拼接成一个大的地图的工作。如下图所示:上面的栅格地图是cartographer得到的整体的地图，下面的两张是其中两个子图对应的栅格地图。

occupancy_grid_node_main中维护的全局地图数据:

  std::map submap_slices_ GUARDED_BY(mutex_);

在构造函数中，地图的生成需要子图序列之间的关系，以及每个子图的数据。订阅子图服务的client_和子图序列的话题关系的submap_list_subscriber_

      client_(node_handle_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(kSubmapQueryServiceName)),
      submap_list_subscriber_(node_handle_.subscribe(kSubmapListTopic, kLatestOnlyPublisherQueueSize,
          boost::function<void(const cartographer_ros_msgs::SubmapList::ConstPtr&)>(
              [this](const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {HandleSubmapList(msg);}))),
      occupancy_grid_publisher_(node_handle_.advertise<::nav_msgs::OccupancyGrid>(
              kOccupancyGridTopic, kLatestOnlyPublisherQueueSize,true /* latched */)),
      occupancy_grid_publisher_timer_(node_handle_.createWallTimer(
                  ::ros::WallDuration(publish_period_sec),&Node::DrawAndPublish, this)) {}

子图序列如下，可以看出子图序列包含所有子图在map坐标系下的位姿，已经完成构建的子图，不再改变submap_version的值也不改变了submap_version:180。如下图中Trajectory 0: 0.180 1.180……，表示0轨迹下的子图。每一次激光数据到达的时候会修改最新的两个子图，例如:submap_index的33和34的submap_version的还会继续更新，直到submap_index为33的地图不再修改而增添新的子图。在序列中也能看出子图的角度也有变化。


子图序列的主题回调函数的任务是将子图的数据存入到submap_slices_。首先判断每个子图的submap_version是否不再更新，当子图存在且内容不再变化，只改变子图在map坐标系下的位姿:submap_slice.pose=ToRigid3d(submap_msg.pose)
关于ActiveSubmaps代码中的注释是这样说的，除了地图在初始化的时候，只有一个子图。其它的时刻，数据都有两个子图插入，一个old submap和一个new submap。 然后当确定数量的数据帧加入后，old submap不再变化，new submap开始初始化。这个时候new submap完成初始化变成old submap，用来完成scan-to-map匹配，也会生成一个new submap。 也就是说有个新老交替的过程用来对接scan-to-map匹配。
在cartographer_ros/cartographer_ros/submap.cc中，使用了FastGunzipString可以看出子图的数据被压缩了，数据量大的情况下是应该的。

  auto response = ::cartographer::common::make_unique();
  response->version = srv.response.submap_version;
  for (const auto& texture : srv.response.textures) {
    std::string compressed_cells(texture.cells.begin(), texture.cells.end());
    std::string cells;
    ::cartographer::common::FastGunzipString(compressed_cells, &cells);
    const int num_pixels = texture.width * texture.height;
    CHECK_EQ(cells.size(), 2 * num_pixels);
    …………………………………………………………………………………………………………………………………………
    }
  response->textures.emplace_back(
        SubmapTexture{intensity, alpha, texture.width, texture.height,
                      texture.resolution, ToRigid3d(texture.slice_pose)});

解压的函数如下，完成数据的SubmapTexture数据的解压缩。

inline void FastGunzipString(const string& compressed, string* decompressed) {
  boost::iostreams::filtering_ostream out;
  out.push(boost::iostreams::gzip_decompressor());
  out.push(boost::iostreams::back_inserter(*decompressed));
  boost::iostreams::write(out, reinterpret_cast<const char*>(compressed.data()),
                          compressed.size());
}

map主题的发布由定时器设定，执行DrawAndPublish,在这个过程中，首先获得由submap_slices_子图组成的整体地图:auto painted_slices=PaintSubmapSlices(&submap_slices_,resolution_);然后发布出去Node::PublishOccupancyGrid。

地图格式

在这个过程中，我们可以单独的获取到每一个子图。每一个子图都是在统一的map坐标系下。如下图，我发布了三个子图的topic:map0，map3，map10。黑色是0表示unknown,白色是254表示free，63表示occupied,其它的在63–>200之间吧。因此在这里分析一下ROS中的地图的数据格式:nav_msgs::OccupancyGrid

std_msgs/Header header    ##
nav_msgs/MapMetaData info ## width,height,resolution,origin
int8[] data               ## 行须优先，以(0,0)左上为origin开始，概率在[0,100],未知为-1
如下:左边为数据存在int8[] data中的index，右边是对应的map坐标系下的坐标。(0,0)为origin
因此递推,可以得到其余各个栅格点的数据。
0 1 2 3 4       (0,0) (0,1) (0,2) (0,3)
5 6 7 8 9       (1,0) (1,1) (1,2) (1,3)
………………………       …………………………………………………………

在使用rosrun map_server map_saver -f filename的时候，执行如下的代码，注意这里保存地图的时候坐标系发生了变换，以左下为origin;ROS map_server，因此可以知道在保存地图的时候做了一次上下对调。使得

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;
    int pixel = (double)(100.0-map->data[i]) *2.54;
    fputc(pixel, out);
  }
}
如下:地图的origin为左下，可递推得到每一栅格的位置
………………………       …………………………………………………………
5 6 7 8 9       (1,0) (1,1) (1,2) (1,3)
0 1 2 3 4       (0,0) (0,1) (0,2) (0,3)

之后在rosrun map_server map_server filename.yaml加载栅格地图的时候，为了还原到之前topic的数据格式，需要将坐标系再次变换，变换为nav_msgs::OccupancyGrid的格式。因此在其代码image_loader.cpp中:

  for(j = 0; j < resp->map.info.height; j++)……
    for (i = 0; i < resp->map.info.width; i++)……
        resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;

  #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))

然后将子图对应的栅格数据转换成sensor_msgs::PointCloud2类型的cloud数据发布出来，如右下图的彩色点云，点云的参考坐标系为map,这样做是为了将栅格地图转换成点云，同时在RVIZ中观察转换的正确性。此时是按照nav_msgs::OccupancyGrid的数据格式了，因此栅格转换成点云的计算如下:

for(c=0;cint y = c / map_resp_.map.info.width;
    int x = c - y * map_resp_.map.info.width;
    double xx = origin[0] + x * res;
    double yy = origin[1] + y * res;

在下面的RVIZ中的显示得到了验证

修改使用rosrun map_server map_server加载各个子图，为了保证能够正常加载，需要将不同的子图的topic作为参数传入，保证topic的frame_id是统一的即可。子图放在一起如下:

保存子图

在cartographer中一旦子图生成，子图内的数据目前不会发生改变，那么怎么保存子图的数据？我在occupancy_grid_node_main中加入了一个服务rosservice call /getsubmap index来完成这个任务。
1. 使用srv文件来完成服务的使用，在cartographer_ros/cartographer_ros_msgs/srv/中添加GetSubmap.srv，如下:

# Get the map as a nav_msgs/OccupancyGrid
int32 submap_index
bool write_map    
 ---
nav_msgs/OccupancyGrid map

在对应的CMakeLists.txt中加入对nav_msgs的依赖，因为使用了nav_msgs/OccupancyGird，包括package.xml中nav_msgs。
2. 在occupancy_grid_node_main中加入advertiseService,对应的服务定义和回调函数:

submap_service_(node_handle_.advertiseService("getsubmap",&Node::submapCallback,this));//回调函数如下
bool Node::submapCallback(cartographer_ros_msgs::GetSubmap::Request &req,
                          cartographer_ros_msgs::GetSubmap::Response &res){
  int submap_index = req.submap_index; 
  auto painted_slices = PaintSubmapSlices(&submap_slices_, resolution_,submap_index);
  nav_msgs::OccupancyGrid occupancy_grid;
  cairo_surface_t *surface = painted_slices.surface.get();
  Eigen::Array2f& origin=painted_slices.origin;
  const int width = cairo_image_surface_get_width(surface);
  const int height = cairo_image_surface_get_height(surface);
  occupancy_grid.header.stamp = ros::Time::now();
  occupancy_grid.header.frame_id = "map";    //所有的子图都要在统一的坐标系下表示
  occupancy_grid.info.map_load_time = ros::Time::now();
  occupancy_grid.info.resolution = resolution_;
  ……………………………………………………………………………………………………………………
  const uint32_t* pixel_data = reinterpret_cast(cairo_image_surface_get_data(surface));
  occupancy_grid.data.reserve(width * height);
  for (int y = height - 1; y >= 0; --y) {
    for (int x = 0; x < width; ++x) {
      const uint32_t packed = pixel_data[y * width + x];
      const unsigned char color = packed >> 16;
      const unsigned char observed = packed >> 8;
      const int value = observed == 0 ? -1
              : ::cartographer::common::RoundToInt((1. - color / 255.) * 100.);
      CHECK_LE(-1, value); CHECK_GE(100, value);
      occupancy_grid.data.push_back(value);
    }
  }
  res.map=occupancy_grid;
  if(true) {   // 默认一直执行保存地图
      std::string mapname_ ="map" ;
      mapname_.append(boost::lexical_cast<std::string>(submap_index));
      ROS_INFO("Received a %d X %d map @ %.3f m/pix", width,height,resolution_);
      std::string mapdatafile = mapname_ + ".pgm";
      ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str());
      FILE* out = fopen(mapdatafile.c_str(), "w");
      if (!out){
        ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str());
        return false; 
      }
      fprintf(out, "P5\n# CREATOR: Map_generator.cpp %.3f m/pix\n%d %d\n255\n",resolution_,width,height);
      // 参考ROS 的map_server保存地图
      ……………………………………………………………………………………………………………………
  }
  return true;
}

同时知道的是if (occupancy_grid_publisher_.getNumSubscribers() == 0) {return;}，因此需要订阅map主题。
3. 修改cartographer/cartographer/io/submap_painter.h(.cpp)中的PaintSubmapSlices函数，增加index用来传递对应的submap_index。以及这个函数调用的需要index参数的函数。

PaintSubmapSlicesResult PaintSubmapSlices(
    std::map<::cartographer::mapping::SubmapId, SubmapSlice>* submaps,
    const double resolution,int index);

子图加载

在bash中加载所有的子图:

#!/bin/bash
source /home/robot/tempRos_ws/devel/setup.bash 
for ((i=0;i<16;i++))
do
    echo $i
#   map_file="/home/robot/Documents/submapPgm/used_submap/map${i}.yaml"
#   echo ${map_file}
#   mapTopic="map${i}"
    rosrun map_server map_server "/home/robot/Documents/submapPgm/used_submap/map${i}.yaml" "map${i}" &
done