(02) Cartographer source code analysis without dead ends-(79) ROS service→subgraph compression and service sending

Explain the summary link of a series of articles about slam: the most complete slam in history starts from scratch , and explain for this column (02) Analysis of Cartographer source code without dead ends-links are as follows:
(02) Cartographer source code analysis without dead ends- (00) Catalog_Latest None Dead corner explanation: https://blog.csdn.net/weixin_43013761/article/details/127350885

$certification{\color{blue}{right below the end of the article Center} provides my \color{red} contact information, \color{blue} clicks on my photo to display WX→official certification}$

I. Introduction

Through the previous blogs, I learned how ROS released the 3D point cloud map, sub-image pose, Landmark, 2D point cloud data, tf, robot tracking frame trajectory release, etc. calculated by Cartographer. But it doesn't explain how the 2D grid map is published to Rviz for display.

This is because the 2D grid map is not published through the topic, but has its own service. In the node.cc constructor, you can see that it has created many services:

	// Step: 2 声明发布对应名字的ROS服务, 并将服务的发布器放入到vector容器中
	service_servers_.push_back(node_handle_.advertiseService(kSubmapQueryServiceName, &Node::HandleSubmapQuery, this))
......
	service_servers_.push_back(node_handle_.advertiseService(kReadMetricsServiceName, &Node::HandleReadMetrics, this));

These services actually execute the corresponding callback function. From the name, we can know that the corresponding callback function of the submap service is Node::HandleSubmapQuery(), and other services will not be explained here for the time being.

This function mainly completes the sending of submaps, but obviously, here are all submaps, not the global map, so where is the global map obtained? For example, the src/cartographer_ros/cartographer_ros/launch/demo_backpack_2d.launch file I run contains backpack_2d.launch, and backpack_2d.launch has the following content:

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

It can be seen that it runs an executable file of cartographer_occupancy_grid_node, which is related to the grid occupancy map. The corresponding source code is located in the src/cartographer_ros/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc file, which will be analyzed later. Of course, the first step is to analyze Node::HandleSubmapQuery().

二、Node::HandleSubmapQuery()

/**
 * @brief 获取对应id轨迹的 索引为submap_index 的submap
 *
 * @param[in] request 获取submap的请求
 * @param[out] response 服务的回应
 * @return true: ROS的service只能返回true, 返回false程序会中断
 */
bool Node::HandleSubmapQuery(
    ::cartographer_ros_msgs::SubmapQuery::Request& request,
    ::cartographer_ros_msgs::SubmapQuery::Response& response) {
    
    
  absl::MutexLock lock(&mutex_);
  map_builder_bridge_.HandleSubmapQuery(request, response);
  return true;
}

This function will not be explained too much, it is to lock and then call the function MapBuilderBridge::HandleSubmapQuery(), which is a submap query service.

3. HandleSubmapQuery()-overall comment

The code is not very complicated, here are the overall comments:

/**
 * @brief 获取对应id轨迹的 索引为 submap_index 的地图的栅格值及其他信息
 * 
 * @param[in] request 轨迹id与submap的index
 * @param[in] response 是否成功
 */
void MapBuilderBridge::HandleSubmapQuery(
    cartographer_ros_msgs::SubmapQuery::Request& request,
    cartographer_ros_msgs::SubmapQuery::Response& response) {
    
    
  cartographer::mapping::proto::SubmapQuery::Response response_proto;
  cartographer::mapping::SubmapId submap_id{
    
    request.trajectory_id,
                                            request.submap_index};
  // 获取压缩后的地图数据
  const std::string error =
      map_builder_->SubmapToProto(submap_id, &response_proto);
  if (!error.empty()) {
    
    
    LOG(ERROR) << error;
    response.status.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
    response.status.message = error;
    return;
  }

  response.submap_version = response_proto.submap_version();

  // 将response_proto中的地图栅格值存入到response中
  for (const auto& texture_proto : response_proto.textures()) {
    
    
    response.textures.emplace_back();
    // 获取response中存储地图变量的引用
    auto& texture = response.textures.back();
    // 对引用的变量进行赋值
    texture.cells.insert(texture.cells.begin(), texture_proto.cells().begin(),
                         texture_proto.cells().end());
    texture.width = texture_proto.width();
    texture.height = texture_proto.height();
    texture.resolution = texture_proto.resolution();
    texture.slice_pose = ToGeometryMsgPose(
        cartographer::transform::ToRigid3(texture_proto.slice_pose()));
  }
  response.status.message = "Success.";
  response.status.code = cartographer_ros_msgs::StatusCode::OK;
}

四、MapBuilder::SubmapToProto()

The specific implementation of this function is as follows:

// 返回压缩后的地图数据
std::string MapBuilder::SubmapToProto(
    const SubmapId& submap_id, proto::SubmapQuery::Response* const response) {
    
    
  // 进行id的检查
  if (submap_id.trajectory_id < 0 ||
      submap_id.trajectory_id >= num_trajectory_builders()) {
    
    
    return "Requested submap from trajectory " +
           std::to_string(submap_id.trajectory_id) + " but there are only " +
           std::to_string(num_trajectory_builders()) + " trajectories.";
  }

  // 获取地图数据
  const auto submap_data = pose_graph_->GetSubmapData(submap_id);
  if (submap_data.submap == nullptr) {
    
    
    return "Requested submap " + std::to_string(submap_id.submap_index) +
           " from trajectory " + std::to_string(submap_id.trajectory_id) +
           " but it does not exist: maybe it has been trimmed.";
  }

  // 将压缩后的地图数据放入response
  submap_data.submap->ToResponseProto(submap_data.pose, response);
  return "";
}

First, check whether the input subgraph id is normal, and if it is not normal, an error will be reported, which tells that only the subgraphs of those trajectories can be selected. Then call pose_graph_->GetSubmapData() to get data from the backend, and the submap data is stored in the backend optimized PoseGraph2D::data::submap_data variable. Then perform data compression, that is, call the function submap_data.submap->ToResponseProto(), which will store the compressed data in the response.

Five, ProbabilityGrid::DrawToSubmapTexture()-submap compression

After entering the Submap2D::ToResponseProto function, you can see that it calls the function code grid()->DrawToSubmapTexture(), which is implemented in the src/cartographer/cartographer/mapping/2d/probability_grid.cc file. Let’s take a look at this implementation of the function.

First of all, the submap will be expanded when it is constructed, and there may be many unknown areas in the expanded map, that is, the grid value is 0.5, which is not very meaningful, so it is only necessary to clip the explored area according to ProbabilityGrid::known_cells_box_ That's it, and then compress the image data. First look at the overall comment of the function, and then analyze it in detail:

// 获取压缩后的地图栅格数据
bool ProbabilityGrid::DrawToSubmapTexture(
    proto::SubmapQuery::Response::SubmapTexture* const texture,
    transform::Rigid3d local_pose) const {
    
    
  Eigen::Array2i offset;
  CellLimits cell_limits;
  // 根据bounding_box对栅格地图进行裁剪
  ComputeCroppedLimits(&offset, &cell_limits);

  std::string cells;
  // 遍历地图, 将栅格数据存入cells
  for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
    
    
    if (!IsKnown(xy_index + offset)) {
    
    
      cells.push_back(0 /* unknown log odds value */);
      cells.push_back(0 /* alpha */);
      continue;
    }
    // We would like to add 'delta' but this is not possible using a value and
    // alpha. We use premultiplied alpha, so when 'delta' is positive we can
    // add it by setting 'alpha' to zero. If it is negative, we set 'value' to
    // zero, and use 'alpha' to subtract. This is only correct when the pixel
    // is currently white, so walls will look too gray. This should be hard to
    // detect visually for the user, though.
    // 我们想添加 'delta'，但使用值和 alpha 是不可能的
    // 我们使用预乘 alpha，因此当 'delta' 为正时，我们可以通过将 'alpha' 设置为零来添加它。 
    // 如果它是负数，我们将 'value' 设置为零，并使用 'alpha' 进行减法。 这仅在像素当前为白色时才正确，因此墙壁看起来太灰。 
    // 但是，这对于用户来说应该很难在视觉上检测到。
    
    // delta处于[-127, 127]
    const int delta =
        128 - ProbabilityToLogOddsInteger(GetProbability(xy_index + offset));
    const uint8 alpha = delta > 0 ? 0 : -delta;
    const uint8 value = delta > 0 ? delta : 0;
    // 存数据时存了2个值, 一个是栅格值value, 另一个是alpha透明度
    cells.push_back(value);
    cells.push_back((value || alpha) ? alpha : 1);
  }

  // 保存地图栅格数据时进行压缩
  common::FastGzipString(cells, texture->mutable_cells());
  
  // 填充地图描述信息
  texture->set_width(cell_limits.num_x_cells);
  texture->set_height(cell_limits.num_y_cells);
  const double resolution = limits().resolution();
  texture->set_resolution(resolution);
  const double max_x = limits().max().x() - resolution * offset.y();
  const double max_y = limits().max().y() - resolution * offset.x();
  *texture->mutable_slice_pose() = transform::ToProto(
      local_pose.inverse() *
      transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));

  return true;
}

$\color{blue}(1)$ First call ComputeCroppedLimits() to obtain the offset translation and size cell_limits of the known area.

$\color{blue}(2)$ Create an instance of std::string cells to store the cut map. XYIndexRangeIterator(cell_limits) is iterated in the source code, which can be understood as xy_index is the pixel coordinate in the cut (new submap), xy_index + offset is the pixel coordinates in the sub-image (old sub-image) before clipping.

$\color{blue}(3)$ Determine whether the pixels in the new submap correspond to the positions in the old submap, whether they have been explored, if not, set the log odds (grid value) and transparency to 0, and add them to the new map cells.

$\color{blue}(4)$ If it has been explored, first assign the probability of its occupation to scale to [-127, 127], and assign it to delta. The larger the value, the greater the probability of being occupied.

$\color{blue}(5)$ For the description of a pixel, use two uint8 to describe, that is, 16 bytes.
The first is the pixel value value, and the second is the transparency alpha. In general, the final effect is as follows:

1. When delta is greater than 0, it means that a positive value needs to be added. At this point, value is set to delta and alpha is set to 0. That is, value represents a positive value to add, and alpha represents a transparency of 0, which is completely opaque.

2. When delta is less than or equal to 0, it means that a negative value needs to be subtracted or no operation is performed. At this point, value is set to 0 and alpha is set to -delta. That is to say, a value of 0 means no operation, and alpha means that the transparency is -delta, that is, the transparency is determined according to the size of the negative value that needs to be subtracted.

According to these settings, the values of value and alpha are as follows:

When delta is greater than 0, value is greater than 0, and alpha is 0.
When delta is less than or equal to 0, value is 0, and alpha is greater than or equal to 0.

For a relatively high occupancy rate, opaque. For low occupancy, about low is about transparent.

$\color{blue}(6)$ After setting the number pixel value and transparency, call common::FastGzipString() for compression, and its internal compression core operation is boost::iostreams::gzip_compressor.

$\color{blue}(7)$ Re-describing the cut map, such as height and width settings (in pixels), resolution, and the maximum values of the x and y axes in the world coordinate system. And set the pose for it, which is the following code:

  const double max_x = limits().max().x() - resolution * offset.y();
  const double max_y = limits().max().y() - resolution * offset.x();
  *texture->mutable_slice_pose() = transform::ToProto(
      local_pose.inverse() *
      transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));

Attention should be paid to this place. First, the initial pose of the submap is based on the local system, which is the world physical unit with max_x and max_y. In addition, the +x axis of the local system is directly in front of the starting position of the robot, and the +y axis is directly to the left of the starting position of the robot. I draw the image as follows: the red
insert image description here
coordinate system is the local system, and the blue coordinate system is the submap system. The pose of the graph in the local system, that is, local_pose in the source code, ② indicates the pose of the slice in the local system, and the origin of the slice should correspond to [resolution offset.y(), resolution offset.y ( )], I don’t understand what the final result of *texture->mutable_slice_pose() is. From the naming point of view, it may be the pose of the slice relative to the subgraph, but what solving process involves limits().max() and max_x , max_y. I feel like I don't know what the final result is. Let’s record it first here, it’s $\color{red}Question 1$

6. Conclusion

The ProbabilityGrid::DrawToSubmapTexture() function returns to Submap2D::ToResponseProto() to MapBuilder::SubmapToProto() to MapBuilderBridge::HandleSubmapQuery(). Going back to this function, we can know that the response_proto after map compression is obtained. For response_proto, it can store multiple texture objects, but only one is stored here. After converting the data of response_proto into response, the function ends, and then Node::HandleSubmapQuery() and the execution ends.

At this point, I have a general understanding of the submap compression compression process. First, cut out the explored areas in the submap, and then compress them. But a question remained.

$\color{red} Question 1:$ The meaning of texture->mutable_slice_pose() in the sample object texture of the proto::SubmapQuery::Response::SubmapTexture* sample object texture of the ProbabilityGrid::DrawToSubmapTexture function in the src/cartographer/cartographer/mapping/2d/probability_grid.cc file is What.