cartographer:occupancy_grid_node_main

cartographeroccupancy_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<SubmapId, SubmapSlice> 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<SubmapTextures>();
  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;c<sizeMap;c++){
    int 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中<depend>nav_msgs</depend>。
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<uint32_t*>(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

cartographer_ros的submap獲取與保存

cartographer:occupancy_grid_node_main

子圖介紹

地圖格式

保存子圖

子圖加載

3D 激光SLAM ->loam_velodyne論文與代碼解析Lidar Odometry and Mapping

基於單應矩陣分解的位姿提取方法

cartographer_ros的submap獲取與保存

ROS Navigation的costmap_2d類繼承關係與實現方法

ROS Navigation的move_base類實現方法

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結