(02)Cartographer源码无死角解析-(83) 系统状态序列化保存、ProtoStreamWriter分析、PoseGraph::ToProto()函数粗解

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

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

一、前言

上一篇博客末尾提到，node.cc 构造函数中，有创建 “write_state” 这个服务，该服务用于保存 Cartographer 系统相关状态，主要包含如下内容：

在这里插入代码片

其保存的方式比较特别，使用 proto 流文件的形式，这样的好处是后续可以通过 proto 直接加载进来，我们知道 proto 提供的接口还是非常全面的。该篇博客主要分析其保存过程，也就是 Node::HandleWriteState() 这个函数，本人搜索这个函数，其只用来创建服务，并没有在其他地方使用到。关于 Node::HandleWriteState() 这个函数的整体可以说十分简单

$\color{blue}(01):$ 其首先调用 map_builder_bridge_.SerializeState() 函数，传递一个文件名以及指定是否包含尚未完成的子图，进行系统状态的保存，随后根据返回的结果打印相关信息。

$\color{blue}(02):$ 如果是3D建图，会把所有的节点的点云数据 ros_point_cloud_map_ 以 pcd 的格式保存在一个 .pcd文件中。ros_point_cloud_map_ 是在发布话题点云数据话题函数 Node::PublishPointCloudMap() 总被赋值的。前面已经分析过。不用多说，Node::HandleWriteState() 中最重要的代码就是对 map_builder_bridge_.SerializeState() 函数的调用。

二、ProtoStreamWriter

$\color{blue}(01):$ 不用多说，Node::HandleWriteState() 中最重要的代码就是对 map_builder_bridge_.SerializeState() 函数的调用，map_builder_bridge_.SerializeState() 又是直接调用了 MapBuilder::SerializeStateToFile() 这个函数。如下：

// 将数据进行压缩,并保存到文件中
bool MapBuilder::SerializeStateToFile(bool include_unfinished_submaps,
                                      const std::string& filename) {
    
    
  io::ProtoStreamWriter writer(filename);
  io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, &writer,
                    include_unfinished_submaps);
  return (writer.Close());
}

其首先构建一个 ProtoStreamWriter 写入流实例 writer，然后把后端 pose_graph_、所有轨迹构建配置参数 all_trajectory_builder_options_、以及是否保存未完成子图 include_unfinished_submaps 都传递给 io::WritePbStream 这个函数。

$\color{blue}(02):$ io::ProtoStreamWriter定义于 src/cartographer/cartographer/io/proto_stream.cc 文件中，其核心函数就是 WriteProto()。该函数接收一个 google::protobuf::Message 类型的数据，然后调用其内部自带的序列化转字符串的函数 proto.SerializeToString(&uncompressed_data)，如下所示：

// 将数据写入文件中
void ProtoStreamWriter::WriteProto(const google::protobuf::Message& proto) {
    
    
  std::string uncompressed_data;
  proto.SerializeToString(&uncompressed_data);
  // 压缩并写入
  Write(uncompressed_data);
}

$\color{blue}(03):$ 从 uncompressed_data 的命名可以知道，其是未压缩的数据，接着把其传递给ProtoStreamWriter::Write()，ProtoStreamWriter::Write() 首先调用 common::FastGzipString() 进行数据压缩，压缩之后之后的数据最终会调用 std::ofstream 实例 out_ 以二进制的形式写如文件，不过写入之时候，先写文件的大小，在写内容。

// 将传入的数据先进行压缩, 再写入到文件中
void ProtoStreamWriter::Write(const std::string& uncompressed_data) {
    
    
  std::string compressed_data;
  // 对数据进行压缩
  common::FastGzipString(uncompressed_data, &compressed_data);
  // 根据数据的size写入文件
  WriteSizeAsLittleEndian(compressed_data.size(), &out_);
  // 将内存中 compressed_data 以二进制的形式写入文件
  out_.write(compressed_data.data(), compressed_data.size());
}

$\color{blue}(04):$ WriteSizeAsLittleEndian 这个函数比较特别，因为大小未 uint64类型，其共64个字节，也就是说，需要把64位都写入，才能保证其正确性，代码如下：

// 写入8个字节的校验位
void WriteSizeAsLittleEndian(uint64 size, std::ostream* out) {
    
    
  for (int i = 0; i != 8; ++i) {
    
    
    out->put(size & 0xff);
    size >>= 8;
  }
}

其把64位的整形看作 8 个字符来，每个字符是八位，其与 0xff 进行按位语，表示每次循环取其右边的八位，写入的到 std::ostream* out 之中，然后 size 再右移八位，避免重复写入。

$\color{blue}(05):$ 通过上面的操作，数据的大小，即 ProtoStreamWriter::Write() 函数中的 compressed_data.size() 就以二进制的形式写入到文件之中了，接着执行 out_.write(compressed_data.data(), compressed_data.size())，把二进制数据写入文件。

$\color{blue}(06):$ 不过上面都是关于 ProtoStreamWriter 的讲解，回到 (1) 中的 MapBuilder::SerializeStateToFile() 函数，其调用 io::ProtoStreamWriter writer(filename) 之后，马上调用了：

io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, &writer,
                    include_unfinished_submaps);

该函数还是比较复杂的。所以没有办法只能先暂时放置一下。

三、WritePbStream

首先该文件位于 mapping_state_serialization.cc 之中，先来看一下函数总体注释，再来进行细节分析：

// 将slam的各个状态与信息写成proto格式
void WritePbStream(
    const mapping::PoseGraph& pose_graph,
    const std::vector<mapping::proto::TrajectoryBuilderOptionsWithSensorIds>&
        trajectory_builder_options,
    ProtoStreamWriterInterface* const writer, bool include_unfinished_submaps) {
    
    
  // Header
  writer->WriteProto(CreateHeader());
  // 位姿图
  writer->WriteProto(
      SerializePoseGraph(pose_graph, include_unfinished_submaps));
  // 参数配置
  writer->WriteProto(SerializeTrajectoryBuilderOptions(
      trajectory_builder_options,
      GetValidTrajectoryIds(pose_graph.GetTrajectoryStates())));

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

  // 传感器数据
  SerializeImuData(pose_graph.GetImuData(), writer);
  SerializeOdometryData(pose_graph.GetOdometryData(), writer);
  SerializeFixedFramePoseData(pose_graph.GetFixedFramePoseData(), writer);
  SerializeLandmarkNodes(pose_graph.GetLandmarkNodes(), writer);
}

writer 就是前面创建的 ProtoStreamWriter 实例，该实例的 WriteProto() 函数已经讲解完了，其就是把一个 Proto 序列化数据转字符串然后压缩，压缩之后以二进制的形式写入，且每次调用 ProtoStreamWriter ::WriteProto() 最前面保存的是压缩后的数据大小。从上面来看 writer->WriteProto() 被调用了了三次，下面就来详细分析一下。

四、writer->WriteProto(CreateHeader())

先来看最简单的 writer->WriteProto(CreateHeader()); ，不多少说，CreateHeader() 返回的数据格式肯定是 google::protobuf::Message 类型：

// 创建消息头
mapping::proto::SerializationHeader CreateHeader() {
    
    
  mapping::proto::SerializationHeader header;
  header.set_format_version(kMappingStateSerializationFormatVersion);
  return header;
}

其就是创建一个头部，头部存储的信息就是kMappingStateSerializationFormatVersion = 2，记录一个版本而已。首先要提及到的是 mapping::proto::SerializationHeader 这个类实现于 install_isolated/include/cartographer/mapping/proto/serialization.pb.h 文件中，而 serialization.pb.h 由 install_isolated/include/cartographer/mapping/proto/serialization.pb.h 文件在编译过程中通过 protoc 指令生成。可见：

// Header of the serialization format. At the moment it only contains the
// version of the format.
message SerializationHeader {
    
    
  uint32 format_version = 1;
}

其就一个 uint32 类型的成员变量，根据 serialization.pb.h 中的定义，可以知道其是继承关系为 SerializationHeader : public ::google::protobuf::Message，故其实例可以作为 writer->WriteProto() 函数的输入参数。

五、 writer->WriteProto(SerializePoseGraph)

前面的示例比较简单，现在来看 WritePbStream() 函数中执行的代码：

  writer->WriteProto(SerializePoseGraph(pose_graph, include_unfinished_submaps));

其通过 SerializePoseGraph() 函数，传入一个 mapping::PoseGraph 类型的实例，以及 include_unfinished_submaps 参数然后构建一个 google::protobuf::Message 类型实例，SerializePoseGraph() 函数实现于 src/cartographer/cartographer/io/internal/mapping_state_serialization.cc 文件中，代码如下所示：

// 将位姿图 序列化到protobuf格式的数据里
SerializedData SerializePoseGraph(const mapping::PoseGraph& pose_graph,
                                  bool include_unfinished_submaps) {
    
    
  SerializedData proto;
  *proto.mutable_pose_graph() = pose_graph.ToProto(include_unfinished_submaps);
  return proto;
}

其主要功能就是把 mapping::PoseGraph 实例对象进行序列化。

$\color{blue}(01):$ 首先构建一个 SerializedData 实例对象，他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;
  }
}

可以看出其包含的数据类型就比较多了，不过这里只用到了其中的 PoseGraph。

$\color{blue}(02):$ 前面提到 writer->WriteProto() 只能接收继承于 google::protobuf::Message 类型的实例数据，SerializedData 当然是符合标准的，但是如何把传入的 mapping::PoseGraph 类型实例 pose_graph 转换成该类型呢？关于 PoseGraph.cc 文件实现的 ToProto 函数个人感觉还是挺复杂的，要获得一个 proto::PoseGraph 类型实例，其需要正常赋值如下 src/cartographer/cartographer/mapping/proto/pose_graph.proto 中定义的如下信息：

message PoseGraph {
    
    
  message Constraint {
    
    
    // Differentiates between intra-submap (where the range data was inserted
    // into the submap) and inter-submap constraints (where the range data was
    // not inserted into the submap).
    enum Tag {
    
    
      INTRA_SUBMAP = 0;
      INTER_SUBMAP = 1;
    }

    SubmapId submap_id = 1;  // Submap ID.
    NodeId node_id = 2;  // Node ID.
    // Pose of the node relative to submap, i.e. taking data from the node frame
    // into the submap frame.
    transform.proto.Rigid3d relative_pose = 3;
    // Weight of the translational part of the constraint.
    double translation_weight = 6;
    // Weight of the rotational part of the constraint.
    double rotation_weight = 7;
    Tag tag = 5;
  }

  message LandmarkPose {
    
    
    string landmark_id = 1;
    transform.proto.Rigid3d global_pose = 2;
  }

  repeated Constraint constraint = 2;
  repeated Trajectory trajectory = 4;
  repeated LandmarkPose landmark_poses = 5;
}

六、PoseGraph::ToProto()

$\color{blue}(01):$ 关于 Proto 编程的语言这里就不作介绍了，现在来详细聊聊 PoseGraph::ToProto() 这个函数的实现，其首先创建 proto::PoseGraph 类型的实例 proto，不用多少这就是最后要返回的实例。接着
构建了一个 std::map<int, proto::Trajectory* const> trajectory_protos 实例，也就是 message PoseGraph 中的成员 trajectory。

$\color{blue}(02):$ 先定义了一个 lambd 函数，且以引用的方式捕获 proto 与 trajectory_protos 这两个变量，该函数接收一个 trajectory_id 参数，根据该参数其首相调用 proto.add_trajectory() 函数获得新增加 PoseGraph ::trajectory 变量的指针，接着把其 trajectory_id 设置好，然后返回该轨迹对应的 trajectory_id 添加到 trajectory_protos 之中。总的来说，该函数就是往空数据 proto 中增加一条 trajectory_id 对应的轨迹，然后获得其指针存储于 trajectory_protos 之中，便于后续操作。

$\color{blue}(03):$ PoseGraph 中可能存在多个子图、多个节点、多条轨迹等，这些都是后续要转换成 proto 格式，然后添加到 proto::PoseGraph 的实例 proto 之中，先来看看其对子图是如何处理的。第一步是通过for循环进行遍历，每次遍历都会调用 $\color{blue}(02):$ 构建的 lambd 函数 trajectory 获得到 proto 中增加轨迹的操作指针 trajectory_proto。下一步就是判断是否保存没有完成的子图，如果不保存则记录在 unfinished_submaps 之中，然后 continue 遍历下一个子图。就是通过 trajectory_proto->add_submap() 操作获取到 trajectory_proto 新增子图的指针，通过该指针对其子图索引进行赋值，同时调用 transform::ToProto() 进行轨迹位姿的变换存出于 trajectory_proto.submap_proto 之中。总的来说，就是把子图的索引，以及位姿添加到其所属轨迹的 trajectory_proto.submap_proto，但是注意并不包含子图数据。

$\color{blue}(04):$ 完成子图的遍历之后，就是对约束的遍历了，其首先进行一个约束的拷贝赋值给 constraints_copy，同时创建一个集合，用于存储节点id，然后启动遍历。如果不保存未完成的子图，且该约束属于为完成的子图，则把其记录在 orphaned_nodes，且从 constraints_copy 中擦除该约束，否者通过 cartographer::mapping::ToProto 把该约束转换成 Proto 格式存储于 proto::PoseGraph 的实例 proto.constraint 之中。

$\color{blue}(05):$ 通过 $\color{blue}(04):$ 知道，orphaned_nodes 中目前存储的是: 不保存未完成子图情况下，属于未完成子图的节点node_id。但是其并不一定就是孤儿节点，因为其还可能同时还与其他的子图，所以也需要擦除。通过该次擦除之后，则能保证 orphaned_nodes 储存的节点只属于未完成的子图。不过比较奇怪，orphaned_nodes 后面没有被使用到，可能就是用于记录调试而已吧，个人不是很理解。

$\color{blue}(06):$ 获取到真正的孤儿节点 orphaned_nodes 之后，且把约束添加到 proto::PoseGraph 的实例 proto.constraint 之中后，是对所有节点的遍历，同样是一个for循环，首先调用 lambd 函数 trajectory 获得节点对应轨迹的 proto::Trajectory 操作指针 trajectory_proto，然后通过 trajectory_proto->add_node() 获得轨迹节点的操作指针，为节点设置 node_index，时间戳，以及节点位姿。需要注意，这里同样不包含一些数据，如节点的雷达数据。

$\color{blue}(07):$ 接着就是对 landmarks 的遍历了，还是同样的配方，所以这里就不再重复了。

$\color{blue} 总结：$ 总的来说 proto::PoseGraph 包含了 proto::Trajectory、proto::PoseGraph_Constraint、proto::Trajectory_Submap、proto::PoseGraph_LandmarkPose 实例对象，其中 proto::Trajectory 又包含了 proto::Trajectory_Submap、proto::Trajectory_Node 实例对象。构建这些实例对象，最终都是调用其对应的 ToProto() 函数如：

proto::PoseGraph_Constraint   对应于  proto::PoseGraph::Constraint ToProto(const PoseGraph::Constraint& constraint)
proto::PoseGraph_LandmarkPose    对应于     proto::Rigid3d ToProto(const transform::Rigid3d& rigid) 
proto::Trajectory_Submap    对应于      proto::Rigid3d ToProto(const transform::Rigid3d& rigid)

如 src/cartographer/cartographer/mapping/proto/trajectory.proto 文件可以知道其包含的 message Submap 只有子图索引号以及 pose，所以其对应于 proto::Rigid3d ToProto(const transform::Rigid3d& rigid) 。

七、总结

到这里为止，关于 mapping_state_serialization.cc 文件中的 WritePbStream 函数只讲解了如下两个部分：

  writer->WriteProto(CreateHeader());
  // 位姿图
  writer->WriteProto(
      SerializePoseGraph(pose_graph, include_unfinished_submaps));

还身下好些调用函数没有讲解，如 SerializeSubmaps()、SerializeTrajectoryNodes()、SerializeTrajectoryData() 等函数，这些我就不再进行分析了，又兴趣的朋友可以自行深入了解一下。

不过通过上面的学习，可以知道复杂的类都有实现 ToProto() 这个函数，我们这里是从 pose_graph 的序列化，其包含的成员太多了，所以过程显得比价复杂。其实可以先分析底层一点的，如接下来:

  SerializeSubmaps(pose_graph.GetAllSubmapData(), include_unfinished_submaps, writer);

这个函数，该函数就是循环遍历每个子图，通过 Submap2D::ToProto() 函数即可把该实例转换成 proto::Submap，通过 writer->WriteProto() 直接保存即可。