(02) Cartographer source code analysis without dead ends-(84) LoadState trajectory map loading, ProtoStreamDeserializer explanation, SerializedData analysis

Explanation of a series of articles about slam summary links: the most complete slam in history starts from scratch , for this column to explain (02) Cartographer source code analysis without dead ends - the link is as follows: (02) Cartographer source code analysis without dead ends -
(00) Catalog_Latest explanation without dead ends: https://blog.csdn.net/weixin_43013761/article/details/127350885 The center at the end of the article provides

$At the bottom of the article, the center provides my contact information. Click on my photo to display W X \to Official certification$

I. Introduction

In the previous blog, the Node::HandleWriteState() function was analyzed. It calls MapBuilderBridge::SerializeState() and then calls MapBuilderStub::SerializeStateToFile() to save the data corresponding to all member variables of MapBuilderBridge. Now go back to the node_main.cc file and find the following code:

  // Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);

  // 如果加载了pbstream文件, 就执行这个函数
  if (!FLAGS_load_state_filename.empty()) {
    
    
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }

  // 使用默认topic 开始轨迹
  if (FLAGS_start_trajectory_with_default_topics) {
    
    
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }

It can be seen from here that if the load_state_filename parameter is passed, the node.LoadState() function will be called. FLAGS_load_state_filename is the .pbstream file saved in the previous blog. FLAGS_load_frozen_state indicates whether to load in a frozen form, and the loaded trajectory (including trajectory nodes, subgraphs, etc.) will not participate in optimization. Here is a review of the startup method of the pure positioning mode track demo:

wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-05-14-44-52.bag
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-27-12-31-41.bag

roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${
    
    HOME}/Downloads/b2-2016-04-05-14-44-52.bag
roslaunch cartographer_ros demo_backpack_2d_localization.launch \
   load_state_filename:=${
    
    HOME}/Downloads/b2-2016-04-05-14-44-52.bag.pbstream \
   bag_filename:=${
    
    HOME}/Downloads/b2-2016-04-27-12-31-41.bag

According to the above code, before starting the pure positioning mode, it loads the map trajectory through node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state), let’s analyze it first.

Two, Node::LoadState()

This function is the opposite of Node::HandleWriteState() introduced in the previous blog, one writes the output .bpstream file, and the other loads the .bpstream file. code show as below:

void Node::LoadState(const std::string& state_filename,
                     const bool load_frozen_state) {
    
    
  absl::MutexLock lock(&mutex_);
  map_builder_bridge_.LoadState(state_filename, load_frozen_state);
  load_state_ = true;
}

void MapBuilderBridge::LoadState(const std::string& state_filename,
                                 bool load_frozen_state) {
    
    
  // Check if suffix of the state file is ".pbstream".
  const std::string suffix = ".pbstream";
  // 检查后缀是否是.pbstream
  CHECK_EQ(state_filename.substr(
               std::max<int>(state_filename.size() - suffix.size(), 0)),
           suffix)
      << "The file containing the state to be loaded must be a "
         ".pbstream file.";
  LOG(INFO) << "Loading saved state '" << state_filename << "'...";
  // 加载文件内容
  cartographer::io::ProtoStreamReader stream(state_filename);
  // 解析数据
  map_builder_->LoadState(&stream, load_frozen_state);
}

Not to mention the rest, for the above code, let’s take a look at the ProtoStreamReader class, which is the opposite of the WritePbStream mentioned in the previous blog, and it will be able to read .PbStream files

三、ProtoStreamReader

This class is defined in the src/cartographer/cartographer/io/proto_stream.cc file, and its core function is ProtoStreamReader::ReadProto() as follows:

bool ProtoStreamReader::ReadProto(google::protobuf::Message* proto) {
    
    
  std::string decompressed_data;
  return Read(&decompressed_data) && proto->ParseFromString(decompressed_data);
}

It first calls the ProtoStreamReader::Read() function to decompress, the decompressed string data is stored in decompressed_data, and then calls the proto->ParseFromString() function to operate. The operation of ProtoStreamReader::Read() is as follows:

bool ProtoStreamReader::Read(std::string* decompressed_data) {
    
    
  uint64 compressed_size;
  // 获取数据的size
  if (!ReadSizeAsLittleEndian(&in_, &compressed_size)) {
    
    
    return false;
  }
  // 根据size生成字符串
  std::string compressed_data(compressed_size, '\0');
  // 读取数据放入compressed_data中
  if (!in_.read(&compressed_data.front(), compressed_size)) {
    
    
    return false;
  }
  // 进行解压
  common::FastGunzipString(compressed_data, decompressed_data);
  return true;
}

It first obtains the first 64 bits of binary data through ReadSizeAsLittleEndian(), that is, the space occupied by binary data. This function is essentially the inverse operation of the WriteSizeAsLittleEndian() function. If the verification is passed, it will decompress and return the data in the form of a string. About proto->ParseFromString(decompressed_data), this function is not much to say, the string data saved by calling proto.SerializeToString(&uncompressed_data) can be directly parsed into google::protobuf::Message type data through this function.

四、ProtoStreamDeserializer

Now go back to the MapBuilderBridge::LoadState() function, after it builds the ProtoStreamReader instance stream, then call the MapBuilder::LoadState() function, and pass the stream as an actual parameter to the formal parameter reader of the function. MapBuilder::LoadState() is the fallback for the MapBuilder::SerializeStateToFile() function.

$\color{blue}(01):$ It first uses the formal parameter reader to construct a ProtoStreamDeserializer instance deserializer. Each call to ProtoStreamReader::ReadProto() can only read one Proto object, but according to the WritePbStream() function of the previous blog, it can be guided that it calls the writer->WriteProto() function multiple times, that is, multiple Proto objects are written, and each Proto is first written in size and then written in data.

$\color{blue}(02):$ ProtoStreamDeserializer can be regarded as the reverse operation of WritePbStream(). WritePbStream() is written in the following order:

	// Header
	writer->WriteProto(CreateHeader());
	// 位姿图
	writer->WriteProto(SerializePoseGraph(pose_graph, include_unfinished_submaps));
	// 参数配置
	writer->WriteProto(SerializeTrajectoryBuilderOptions(trajectory_builder_options,GetValidTrajectoryIds(pose_graph.GetTrajectoryStates())));
	......

In addition to the above three Protos, a lot of content is also written, such as all subgraphs, radar data and trajectory nodes, IMU, odometer and other data. Of course, when loading a .pbstream file, it is not necessary to load all of them. It should be noted that each call to writer->WriteProto() is to write a Proto object, first save the size of the object, and then save the serialized data. The order of writing and reading needs to be consistent, so the order of reading data in the constructor of ProtoStreamDeserializer is as follows:

	IsVersionSupported(header_) 
	ReadNextSerializedData(&pose_graph_) //从pbstream中获取位姿图
	ReadNextSerializedData(&all_trajectory_builder_options_) // 从pbstream中获取配置文件

These three functions are ultimately called to reader_->ReadProto(). I won't be long-winded anymore.

$\color{blue}(03):$ Knowing this, let's go back to the MapBuilder::LoadState() function. From the above, we can know that the deserializer variable contains header_, pose_graph_, all_trajectory_builder_options_. It will be assigned to pose_graph_proto and all_builder_options_proto respectively, but it is Proto type data. The following is to load these data into its corresponding class instance, such as MapBuilder::pose_graph_. The loading process can be said to be very complicated. Now let's talk about MapBuilder::LoadState() in general.

5. MapBuilder::LoadState() - initial addition

$\color{blue}(01):$ First get pose_graph_proto and all_builder_options_proto from the deserializer instance. Next is the traversal of the trajectory in pose_graph_proto. First get the trajectory_proto, and then get the sensor and trajectory configuration corresponding to the trajectory through all_builder_options_proto.options_with_sensor_ids(i). Mainly check the file src/cartographer/cartographer/mapping/proto/trajectory_builder_options.proto, as shown in the following code:

message TrajectoryBuilderOptionsWithSensorIds {
    
    
  repeated SensorId sensor_id = 1;
  TrajectoryBuilderOptions trajectory_builder_options = 2;
}

message AllTrajectoryBuilderOptions {
    
    
  repeated TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids = 1;
}

all_builder_options_proto is an instance of AllTrajectoryBuilderOptions, then options_with_sensor_ids of course corresponds to the TrajectoryBuilderOptionsWithSensorIds array.

$\color{blue}(02):$
According to the estimated configuration information options_with_sensor_ids_proto (including sensor id and trajectory configuration parameters), add a new trajectory by calling the AddTrajectoryForDeserialization() function and obtain a new new_trajectory_id, which is stored in trajectory_remapping with the previous trajectory_proto.trajectory_id() and modify the trajectory idin trajectory_proto to new_ trajectory_id

$\color{blue}(03):$ According to the parameter load_frozen_state configuration track whether to freeze. Freezing means not participating in optimization. In this way, the traversal of the track in pose_graph_proto is completed.

$\color{blue}(04):$ Then there is another for loop, which is the processing of constraints, modifying the trajectories of all constrained submaps and node nodes to new_trajectory_id.

$\color{blue}(05):$ After the constraints are processed, there are two nested loops, the outer layer is the traversal of the trajectory, and the inner layer is the traversal of the trajectory subgraphs. In general, it is to traverse all the subgraphs. Its purpose is to store the pose corresponding to each submap in MapById<SubmapId, transform::Rigid3d> submap_poses;.

$\color{blue}(06):$ It is easy to think that after the subgraph of the trajectory is processed, the trajectory nodes are processed, and two for loops are nested to traverse again, which is to traverse all the nodes of all the trajectories. Store the node pose of node_proto in the variable MapById<NodeId, transform::Rigid3d> node_poses.

$\color{blue}(07):$ The next step is to get the landmark through pose_graph_proto.landmark_poses(), and then add it to the back-end pose graph by calling pose_graph_->SetLandmarkPose() in a loop. Then it is judged whether it is a 3D trajectory. If so, print: "The pbstream file contains a subgraph without a rotation histogram. You can use the 'pbstream migrate' tool for conversion. For details, please refer to the Cartographer document."

$\color{blue} Summary:$ Through the introduction of the appeal, the most important part of LoadState() has been completed. To load the previous .dpstream file, these things are essential: according to the configuration of the trajectory and sensors in the .dpstream, create a new trajectory, and load the subgraph pose and trajectory nodes.

6. MapBuilder::LoadState()-subsequent addition

$\color{blue}(01):$ Followed by a while loop, each loop will call proto.data_case() to judge the read proto data type, mainly as follows:

enum DataCase {
    
    
    kPoseGraph = 1,
    kAllTrajectoryBuilderOptions = 2,
    kSubmap = 3,
    kNode = 4,
    kTrajectoryData = 5,
    kImuData = 6,
    kOdometryData = 7,
    kFixedFramePoseData = 8,
    kLandmarkData = 9,
    DATA_NOT_SET = 0,
  };

$\color{blue}(02):$ With the inconsistency of the data type, the executed code is also different. For example, the read data type is SerializedData::kPoseGraph and SerializedData::kAllTrajectoryBuilderOptions:

      case SerializedData::kPoseGraph:
        LOG(ERROR) << "Found multiple serialized `PoseGraph`. Serialized "
                      "stream likely corrupt!.";
        break;
      case SerializedData::kAllTrajectoryBuilderOptions:
        LOG(ERROR) << "Found multiple serialized "
                      "`AllTrajectoryBuilderOptions`. Serialized stream likely "
                      "corrupt!.";
        break;

Can see it or output an error message, why then? Because the previous PoseGraph and AllTrajectoryBuilderOptions have been read but processed, and if they are read again, it means that they have been stored multiple times, so an error is reported. Then as follows, if the subgraph is read:

      case SerializedData::kSubmap: {
    
    
        // 为submap设置新的轨迹id
        proto.mutable_submap()->mutable_submap_id()->set_trajectory_id(
            trajectory_remapping.at(
                proto.submap().submap_id().trajectory_id()));
        const SubmapId submap_id(proto.submap().submap_id().trajectory_id(),
                                 proto.submap().submap_id().submap_index());
        // 将submap添加到位姿图中
        pose_graph_->AddSubmapFromProto(submap_poses.at(submap_id),
                                        proto.submap());
        break;
      }

Then set a new trajectory_id for the subgraph, and add the subgraph to the backend pose graph pose_graph_ through pose_graph_->AddSubmapFromProto.

$\color{blue}(03):$ It should be noted that, in the introduction of MapBuilder::LoadState()-preliminary addition, only the submap pose submap_pose and node pose node_poses are obtained. So the above code uses submap_poses.at(submap_id). When adding nodes here, node_poses will also be used to get the node pose:

      case SerializedData::kNode: {
    
    
        // 为node_id设置新的轨迹id
        proto.mutable_node()->mutable_node_id()->set_trajectory_id(
            trajectory_remapping.at(proto.node().node_id().trajectory_id()));
        const NodeId node_id(proto.node().node_id().trajectory_id(),
                             proto.node().node_id().node_index());
        const transform::Rigid3d& node_pose = node_poses.at(node_id);
        // 将node_pose添加到位姿图中
        pose_graph_->AddNodeFromProto(node_pose, proto.node());
        break;
      }

It should be noted that the submaps and nodes obtained by proto.submap() and proto.node() contain specific data, as follows:

==================================================================================================================================
message Submap2D {
    
    
  transform.proto.Rigid3d local_pose = 1;
  int32 num_range_data = 2;
  bool finished = 3;
  Grid2D grid = 4;
}
==================================================================================================================================
message TrajectoryNodeData {
    
    
  int64 timestamp = 1;
  transform.proto.Quaterniond gravity_alignment = 2;
  sensor.proto.CompressedPointCloud
      filtered_gravity_aligned_point_cloud = 3;
  sensor.proto.CompressedPointCloud high_resolution_point_cloud = 4;
  sensor.proto.CompressedPointCloud low_resolution_point_cloud = 5;
  repeated float rotational_scan_matcher_histogram = 6;
  transform.proto.Rigid3d local_pose = 7;
}

message Node {
    
    
  NodeId node_id = 1;
  TrajectoryNodeData node_data = 5;
}
==================================================================================================================================

It can be seen that it contains grid map and radar point cloud data respectively.

$\color{blue}(04):$ Later, different data types will be loaded according to different situations, such as SerializedData::kImuData, SerializedData::kOdometryData, SerializedData::kLandmarkData, etc., but there is a strange problem here, that is, how to directly reach the type of data read from .pdstraam. First look at the SerializedData class, which is located in the src/cartographer/cartographer/mapping/proto/serialization.proto file:

message SerializedData {
    
    
  oneof data {
    
    
    PoseGraph pose_graph = 1;
    AllTrajectoryBuilderOptions all_trajectory_builder_options = 2;
    Submap submap = 3;
    Node node = 4;
    TrajectoryData trajectory_data = 5;
    ImuData imu_data = 6;
    OdometryData odometry_data = 7;
    FixedFramePoseData fixed_frame_pose_data = 8;
    LandmarkData landmark_data = 9;
  }
}

From here, we can know that SerializedData must be one of them, so how to use it? Then you have to recall the WritePbStream() function:

	......
	// 所有的submap
	SerializeSubmaps(pose_graph.GetAllSubmapData(), include_unfinished_submaps,
	                 writer);
	// 雷达数据, 前端后端的位姿, 时间戳
	SerializeTrajectoryNodes(pose_graph.GetTrajectoryNodes(), writer);
	// fixed_frame_origin_in_map
	SerializeTrajectoryData(pose_graph.GetTrajectoryData(), writer);
	.....

Need to pay attention to functions like Serialize..., such as SerializeSubmaps:

void SerializeSubmaps(
    const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data,
    bool include_unfinished_submaps, ProtoStreamWriterInterface* const writer) {
    
    
  // Next serialize all submaps.
  for (const auto& submap_id_data : submap_data) {
    
    
    if (!include_unfinished_submaps &&
        !submap_id_data.data.submap->insertion_finished()) {
    
    
      continue;
    }
    SerializedData proto;
    auto* const submap_proto = proto.mutable_submap();
    *submap_proto = submap_id_data.data.submap->ToProto(
        /*include_probability_grid_data=*/true);
    submap_proto->mutable_submap_id()->set_trajectory_id(
        submap_id_data.id.trajectory_id);
    submap_proto->mutable_submap_id()->set_submap_index(
        submap_id_data.id.submap_index);
    writer->WriteProto(proto);
  }
}

You can see that it first constructs a SerializedData proto type of data, and in the process of looping into the subgraph, notice that the code is as follows:

auto* const submap_proto = proto.mutable_submap();

It only calls SerializedData::mutable_submap(), that is, sets its data type to Submap. Because when the Submap instance is constructed again, the others will be cleared.

6. Conclusion

Through this blog, for the node_main.cc file, called in

  // 如果加载了pbstream文件, 就执行这个函数
  if (!FLAGS_load_state_filename.empty()) {
    
    
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }

Function, it can be said that the explanation is completed, and its main function is to load trajectory, node, subgraph, constraint, IMU, GPS and other data according to the saved .dpstream file. At the same time a new track will be created. Then it calls:

  // 使用默认topic 开始轨迹
  if (FLAGS_start_trajectory_with_default_topics) {
    
    
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }

However, the processing here is a little different from the previous ones. Usually, the .dpstream file is loaded and run in pure positioning mode. The next blog will analyze the process of pure positioning mode.