(02)Cartographer源码无死角解析-(84) LoadState轨迹地图加载、ProtoStreamDeserializer讲解、SerializedData 分析

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

上一篇博客中，对于 Node::HandleWriteState() 函数进行了分析，其通过调用 MapBuilderBridge::SerializeState() 进而调用到 MapBuilderStub::SerializeStateToFile() 把 MapBuilderBridge 所有成员变量对应的数据都保存下来。现在回到 node_main.cc 文件，找到如下代码：

  // 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);
  }

从这里可以看到，如果传递了 load_state_filename 这个参数，则会调用 node.LoadState() 这个函数，FLAGS_load_state_filename 就是上一篇博客保存的 .pbstream 文件，FLAGS_load_frozen_state 表示是否以冻结的形式进行加载，加载的轨迹(含轨迹的节点、子图等)都不会参与优化。这里再回顾一下纯定位模式轨迹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

根据上面的代码之后在进行纯定位模式启动之前，其通过 node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state) 加载地图轨迹等，先来分析一下。

二、Node::LoadState()

该函数与上一篇博客介绍的 Node::HandleWriteState() 是相反的操作，一个写入输出 .bpstream 文件，一个加载 .bpstream 文件。代码如下：

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);
}

其他的就不说了，对于上面的代码，来看看 ProtoStreamReader 这个类，其与上一篇博客提到的 WritePbStream 作用是相反的，其会能够对 .PbStream 的文件进行读取

三、ProtoStreamReader

该类定义于 src/cartographer/cartographer/io/proto_stream.cc 文件之中，其最核心的函数就是 ProtoStreamReader::ReadProto() 如下所示：

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

其首先调用 ProtoStreamReader::Read() 函数进行解压，解压获得字符串数据存放于 decompressed_data 之中，随后调用 proto->ParseFromString() 函数进行操作。关于 ProtoStreamReader::Read() 的操作如下：

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;
}

其首先通过 ReadSizeAsLittleEndian() 获得二进制数据前64位，也就是二进制数据占用空间大小，该函数本质上就是 WriteSizeAsLittleEndian() 函数的逆操作。如果验证通过，则解压且以字符串的形式返回数据。关于 proto->ParseFromString(decompressed_data)，这函数就不多说了，调用 proto.SerializeToString(&uncompressed_data) 保存的字符串数据，都可以通过该函数直接解析成 google::protobuf::Message 类型数据。

四、ProtoStreamDeserializer

现在回到 MapBuilderBridge::LoadState() 函数，在其构建好 ProtoStreamReader 实例 stream 之后，接着调用了 MapBuilder::LoadState() 函数，且把 stream 作为实参数传递给该函数的形参 reader。MapBuilder::LoadState() 是 MapBuilder::SerializeStateToFile() 函数的返向操作。

$\color{blue}(01):$ 其首先利用形参 reader构建一个 ProtoStreamDeserializer 实例 deserializer。每次调用 ProtoStreamReader::ReadProto() 只能读取一个 Proto 对象，但是根据上一篇博客的 WritePbStream() 函数可以指导，其调用了多次 writer->WriteProto() 函数，即写入了多个 Proto 对象，且其每个 Proto 都是先写入大小，再写入数据的。

$\color{blue}(02):$ 可以把 ProtoStreamDeserializer 看作 WritePbStream() 的反向操作，WritePbStream() 写如类容先后顺序如下：

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

除上述三个Proto之外，还写入了很多内容，如所有子图、雷达数据与轨迹节点、IMU、里程计等数据。当然，加载 .pbstream 文件时，并不需要全部加载，需要注意的是，每次调用 writer->WriteProto() 是写入一个 Proto 对象，先保存该对象占用大小，接着再保存序列化数据。写入与读取的顺序需要保持一致，所以在ProtoStreamDeserializer 的构造函数中读取数据的顺序如下：

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

这三个函数最终都是调用到 reader_->ReadProto()。就不再啰嗦了。

$\color{blue}(03):$ 了解到这里我们再回到 MapBuilder::LoadState() 函数，通过上述可以知道，deserializer 变量中包含了 header_，pose_graph_，all_trajectory_builder_options_。其会分别赋值给 pose_graph_proto、all_builder_options_proto，但是其为 Proto 类型的数据，下面就是要把在这些数据都加载到其对应的类实例之中，如 MapBuilder::pose_graph_。加载的过程可以说是十分复杂的，现在就来大致说一下 MapBuilder::LoadState()。

五、MapBuilder::LoadState()-初步添加

$\color{blue}(01):$ 首先从 deserializer 实例获取到 pose_graph_proto 与 all_builder_options_proto。接着就是对 pose_graph_proto 中轨迹的遍历，先获去其中的 trajectory_proto，然后通过all_builder_options_proto.options_with_sensor_ids(i) 得到轨迹对应的传感器与轨迹配置，主要查看 src/cartographer/cartographer/mapping/proto/trajectory_builder_options.proto 这个文件，可见如下代码：

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 就是 AllTrajectoryBuilderOptions 的实例，那么 options_with_sensor_ids 当然对应于 TrajectoryBuilderOptionsWithSensorIds 数组了。

$\color{blue}(02):$ 根据估计的配置信息 options_with_sensor_ids_proto(含传感器 id 以及轨迹配置参数) ，通过调用 AddTrajectoryForDeserialization() 函数添加一条新的轨迹且获的新的 new_trajectory_id，其与之前的 trajectory_proto.trajectory_id() 存储于 trajectory_remapping 之中且修改 trajectory_proto 中的轨迹
id 为 new_trajectory_id

$\color{blue}(03):$ 根据参数 load_frozen_state 配置轨迹是否冻结。冻结则表示不参与优化。这样，对于 pose_graph_proto 中轨迹的遍历就完成了。

$\color{blue}(04):$ 接着又是一个for循环，其是对约束的处理，把所有约束的子图 submap、节点 node 的所属轨迹都修改成 new_trajectory_id。

$\color{blue}(05):$ 处理完约束之后，接着是两个嵌套的循环，外层是对轨迹的遍历，内层是对轨迹子图的遍历，总的来说，就是遍历所有的所有子图。其目的是为了往 MapById<SubmapId, transform::Rigid3d> submap_poses; 中存储每个子图对应的位姿。

$\color{blue}(06):$ 容易想到，轨迹的子图处理完了就是处理轨迹节点了，再次两个for循环进行嵌套遍历，其就是遍历所有轨迹的所有节点。把 node_proto 的节点位姿存储于变量 MapById<NodeId, transform::Rigid3d> node_poses之中。

$\color{blue}(07):$ 接着就是通过 pose_graph_proto.landmark_poses() 获得 landmark，接着通过循环调用 pose_graph_->SetLandmarkPose() 把其添加到后端位姿图之中。随后进行了是否为 3D轨迹的判断，如是，则打印：“pbstream文件中包含没有旋转直方图的子图。可以使用’pbstream migrate’工具进行转换，详细信息请参考Cartographer文档。”

$\color{blue}总结：$ 通过上诉简介，对于 LoadState() 最主要部分已经完成，加载之前的 .dpstream 文件，这几个东西是必不可少的：根据 .dpstream 中轨迹与传感器的配置，新建轨迹，加载子图位姿与轨迹节点。

六、MapBuilder::LoadState()-后续添加

$\color{blue}(01):$ 后面是一个while循环，每次循环都是会调用 proto.data_case() 判断一下读取到的 proto 数据类型，主要有如下几种：

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):$ 随着数据类型的不一致，执行的代码也不一样，比如，读取到的数据类型为 SerializedData::kPoseGraph 与 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;

可以看到其或输出错误信息，为什那么呢？因为前面 PoseGraph 与 AllTrajectoryBuilderOptions 已经读取却处理过了，再读取到则说明其存储了多次，所以报错。接着如下，如果读取到的是子图：

      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;
      }

那么就为子图设置新的 trajectory_id，且将该子图通过 pose_graph_->AddSubmapFromProto 添加至后端位姿图 pose_graph_ 之中。

$\color{blue}(03):$ 需要注意一点的是，再前面 MapBuilder::LoadState()-初步添加介绍中，只获取到了子图位姿 submap_pose，以及节点位姿 node_poses。所以上面的代码使用到了 submap_poses.at(submap_id)。这里添加节点的时候，也会使用到 node_poses 获取到节点位姿：

      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;
      }

需要注意的是 proto.submap() 与 proto.node() 获取到的子图与节点都是包含具体数据的，如下：

==================================================================================================================================
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;
}
==================================================================================================================================

可知其分别包含了栅格地图与雷达点云数据。

$\color{blue}(04):$ 后面还会根据不同的情况加载不同的数据类型，如 SerializedData::kImuData、SerializedData::kOdometryData、SerializedData::kLandmarkData 等，但是这里有个奇怪的问题，那就是从 .pdstraam 读取的数据，如何直达其所属类型的。首先来看一下 SerializedData 这个类，其位于 src/cartographer/cartographer/mapping/proto/serialization.proto 文件之中：

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;
  }
}

从这里可以知道 SerializedData 必定是其中的一类，那么又是如何利用起来的呢？那就得回忆一下 WritePbStream()这个函数了：

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

需要注意 Serialize… 这样的函数，如 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);
  }
}

可以看到其先构建一个 SerializedData proto 类型的数据，其循环写入子图过程中，注意到代码如下：

auto* const submap_proto = proto.mutable_submap();

其只对 SerializedData ::mutable_submap() 进行了调用，也就是设置可其数据类型为 Submap 。因为再构建 Submap 实例的时候，其他的会被清空。

六、结语

通过该篇博客，对于 node_main.cc 文件，中调用的

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

函数，可以说讲解完成了，其主要就是根据保存的 .dpstream 文件加载轨迹、节点、子图、约束、IMU、GPS 等数据。同时会创建出一条新的轨迹。接着其就调用:

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

不过处理，这里的调用和前面就有一些不一样了，通常加载 .dpstream 文件，都是以纯定位模式运行的，下篇博客就来好好分析一下纯定位模式的流程。