Cartographer源码阅读2D&3D-Pbstream读写

Cartographer源码阅读2D&3D-Pbstream读写

写入proto

查看protobuf官方文档：

Protobuf文档

需要自定义需要写入和读取的message即可。定义好.proto文件，在编译的阶段生成相应的.pb.h文件，通过调用这些文件，保存和读取proto文件。

pbstream文件保存的内容

保存pbstream文件是通过服务调用的，接口函数为node.cc文件中：

bool Node::HandleWriteState(
    ::cartographer_ros_msgs::WriteState::Request& request,
    ::cartographer_ros_msgs::WriteState::Response& response) {
  carto::common::MutexLocker lock(&mutex_);
  // 保存pbstream文件
  if (map_builder_bridge_.SerializeState(request.filename)) {
    response.status.code = cartographer_ros_msgs::StatusCode::OK;
    response.status.message = "State written to '" + request.filename + "'.";
  } else {
    response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
    response.status.message = "Failed to write '" + request.filename + "'.";
  }
  return true;
}

该函数调用的是map_builder_bridge.cc的函数：

bool MapBuilderBridge::SerializeState(const std::string& filename) {
  // 构造写入器
  cartographer::io::ProtoStreamWriter writer(filename);
  // 写入数据
  map_builder_->SerializeState(&writer);
  return writer.Close();
}

写入pbstream的函数在map_builder.cc文件中：

void MapBuilder::SerializeState(io::ProtoStreamWriterInterface* const writer) {
  io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, writer);
}

void WritePbStream(
    const mapping::PoseGraph& pose_graph,
    const std::vector<mapping::proto::TrajectoryBuilderOptionsWithSensorIds>&
        trajectory_builder_options,
    ProtoStreamWriterInterface* const writer) {
  // 写入header
  writer->WriteProto(CreateHeader());
  // 写入pose_graph
  writer->WriteProto(SerializePoseGraph(pose_graph));
  // 写入trajectory的参数
  writer->WriteProto(
      SerializeAllTrajectoryBuilderOptions(trajectory_builder_options));
  // 写入submap
  SerializeSubmaps(pose_graph.GetAllSubmapData(), writer);
  // 写入trajectory node
  SerializeTrajectoryNodes(pose_graph.GetTrajectoryNodes(), writer);
  // 写入TrajectoryData
  SerializeTrajectoryData(pose_graph.GetTrajectoryData(), writer);
  // 写入imu数据
  SerializeImuData(pose_graph.GetImuData(), writer);
  // 写入轮速计数据
  SerializeOdometryData(pose_graph.GetOdometryData(), writer);
  // 写入fix pose
  SerializeFixedFramePoseData(pose_graph.GetFixedFramePoseData(), writer);
  // 写入landmark
  SerializeLandmarkNodes(pose_graph.GetLandmarkNodes(), writer);
}

mapping::proto::SerializationHeader CreateHeader() {
  mapping::proto::SerializationHeader header;
  header.set_format_version(kMappingStateSerializationFormatVersion);
  return header;
}

写入pose_graph：

proto::PoseGraph PoseGraph::ToProto() const {
  proto::PoseGraph proto;

  std::map<int, proto::Trajectory* const> trajectory_protos;
  // 构造Lambda表达式
  const auto trajectory = [&proto, &trajectory_protos](
                              const int trajectory_id) -> proto::Trajectory* {
    if (trajectory_protos.count(trajectory_id) == 0) {
      auto* const trajectory_proto = proto.add_trajectory();
      trajectory_proto->set_trajectory_id(trajectory_id);
      CHECK(trajectory_protos.emplace(trajectory_id, trajectory_proto).second);
    }
    return trajectory_protos.at(trajectory_id);
  };

  for (const auto& node_id_data : GetTrajectoryNodes()) {
    CHECK(node_id_data.data.constant_data != nullptr);
    // 写入node
    auto* const node_proto =
        trajectory(node_id_data.id.trajectory_id)->add_node();
    // node 的id
    node_proto->set_node_index(node_id_data.id.node_index);
    // node的时间
    node_proto->set_timestamp(
        common::ToUniversal(node_id_data.data.constant_data->time));
    // node 的 global pose
    *node_proto->mutable_pose() =
        transform::ToProto(node_id_data.data.global_pose);
  }

  for (const auto& submap_id_data : GetAllSubmapData()) {
    CHECK(submap_id_data.data.submap != nullptr);
    // 写入submap
    auto* const submap_proto =
        trajectory(submap_id_data.id.trajectory_id)->add_submap();
    // submap的id
    submap_proto->set_submap_index(submap_id_data.id.submap_index);
    // submap的global pose
    *submap_proto->mutable_pose() =
        transform::ToProto(submap_id_data.data.pose);
  }

  auto constraints_copy = constraints();
  proto.mutable_constraint()->Reserve(constraints_copy.size());
  for (const auto& constraint : constraints_copy) {
    // 写入约束
    *proto.add_constraint() = cartographer::mapping::ToProto(constraint);
  }

  auto landmarks_copy = GetLandmarkPoses();
  proto.mutable_landmark_poses()->Reserve(landmarks_copy.size());
  for (const auto& id_pose : landmarks_copy) {
    // 写入landmark
    auto* landmark_proto = proto.add_landmark_poses();
    // landmark的id
    landmark_proto->set_landmark_id(id_pose.first);
    // landmark的pose
    *landmark_proto->mutable_global_pose() = transform::ToProto(id_pose.second);
  }
  return proto;
}

写入trajectory的参数

SerializedData SerializeAllTrajectoryBuilderOptions(
    const std::vector<mapping::proto::TrajectoryBuilderOptionsWithSensorIds>&
        trajectory_builder_options) {
  SerializedData proto;
  *proto.mutable_all_trajectory_builder_options() =
      CreateAllTrajectoryBuilderOptionsProto(trajectory_builder_options);
  return proto;
}

mapping::proto::AllTrajectoryBuilderOptions
CreateAllTrajectoryBuilderOptionsProto(
    const std::vector<mapping::proto::TrajectoryBuilderOptionsWithSensorIds>&
        all_options_with_sensor_ids) {
  mapping::proto::AllTrajectoryBuilderOptions all_options_proto;
  for (const auto& options_with_sensor_ids : all_options_with_sensor_ids) {
    *all_options_proto.add_options_with_sensor_ids() = options_with_sensor_ids;
  }
  CHECK_EQ(all_options_with_sensor_ids.size(),
           all_options_proto.options_with_sensor_ids_size());
  return all_options_proto;
}

写入submap：

void SerializeSubmaps(
    const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data,
    ProtoStreamWriterInterface* const writer) {
  SerializedData proto;
  // Next serialize all submaps.
  for (const auto& submap_id_data : submap_data) {
    auto* const submap_proto = proto.mutable_submap();
    // submap的trajectory id
    submap_proto->mutable_submap_id()->set_trajectory_id(
        submap_id_data.id.trajectory_id);
    // submap的map index
    submap_proto->mutable_submap_id()->set_submap_index(
        submap_id_data.id.submap_index);
    // 写入submap数据
    submap_id_data.data.submap->ToProto(submap_proto,
                                        /*include_probability_grid_data=*/true);
    writer->WriteProto(proto);
  }
}

void Submap2D::ToProto(proto::Submap* const proto,
                       bool include_probability_grid_data) const {
  // 写入proto
  auto* const submap_2d = proto->mutable_submap_2d();
  // submap的Local pose
  *submap_2d->mutable_local_pose() = transform::ToProto(local_pose());
  // submap的帧的数量
  submap_2d->set_num_range_data(num_range_data());
  // submap的状态是否finished
  submap_2d->set_finished(finished());
  if (include_probability_grid_data) {
    CHECK(grid_);
    // 写入submap的网格数据
    *submap_2d->mutable_grid() = grid_->ToProto();
  }
}

proto::Grid2D Grid2D::ToProto() const {
  proto::Grid2D result;
  // 写入limits
  *result.mutable_limits() = mapping::ToProto(limits_);
  result.mutable_cells()->Reserve(correspondence_cost_cells_.size());
  for (const auto& cell : correspondence_cost_cells_) {
    // 写入概率
    result.mutable_cells()->Add(cell);
  }
  CHECK(update_indices().empty()) << "Serializing a grid during an update is "
                                     "not supported. Finish the update first.";
  if (!known_cells_box().isEmpty()) {
    // 写入box
    auto* const box = result.mutable_known_cells_box();
    box->set_max_x(known_cells_box().max().x());
    box->set_max_y(known_cells_box().max().y());
    box->set_min_x(known_cells_box().min().x());
    box->set_min_y(known_cells_box().min().y());
  }
  // 写入最大最小free概率
  result.set_min_correspondence_cost(min_correspondence_cost_);
  result.set_max_correspondence_cost(max_correspondence_cost_);
  return result;
}

写入trajectory node：

void SerializeTrajectoryNodes(
    const MapById<NodeId, TrajectoryNode>& trajectory_nodes,
    ProtoStreamWriterInterface* const writer) {
  SerializedData proto;
  for (const auto& node_id_data : trajectory_nodes) {
    auto* const node_proto = proto.mutable_node();
    // node的trajectory id
    node_proto->mutable_node_id()->set_trajectory_id(
        node_id_data.id.trajectory_id);
    // node的node index
    node_proto->mutable_node_id()->set_node_index(node_id_data.id.node_index);
    // node的数据
    *node_proto->mutable_node_data() =
        ToProto(*node_id_data.data.constant_data);
    writer->WriteProto(proto);
  }
}

写入TrajectoryData：

void SerializeTrajectoryData(
    const std::map<int, PoseGraphInterface::TrajectoryData>&
        all_trajectory_data,
    ProtoStreamWriterInterface* const writer) {
  SerializedData proto;
  for (const auto& trajectory_data : all_trajectory_data) {
    auto* const trajectory_data_proto = proto.mutable_trajectory_data();
    // trajectory的轨迹
    trajectory_data_proto->set_trajectory_id(trajectory_data.first);
    // 轨迹的重力方向
    trajectory_data_proto->set_gravity_constant(
        trajectory_data.second.gravity_constant);
    // imu外参
    *trajectory_data_proto->mutable_imu_calibration() = transform::ToProto(
        Eigen::Quaterniond(trajectory_data.second.imu_calibration[0],
                           trajectory_data.second.imu_calibration[1],
                           trajectory_data.second.imu_calibration[2],
                           trajectory_data.second.imu_calibration[3]));
    if (trajectory_data.second.fixed_frame_origin_in_map.has_value()) {
      // fix pose
      *trajectory_data_proto->mutable_fixed_frame_origin_in_map() =
          transform::ToProto(
              trajectory_data.second.fixed_frame_origin_in_map.value());
    }
    writer->WriteProto(proto);
  }
}

写入imu数据：

void SerializeImuData(const sensor::MapByTime<sensor::ImuData>& all_imu_data,
                      ProtoStreamWriterInterface* const writer) {
  SerializedData proto;
  for (const int trajectory_id : all_imu_data.trajectory_ids()) {
    // 遍历所有轨迹的imu数据
    for (const auto& imu_data : all_imu_data.trajectory(trajectory_id)) {
      auto* const imu_data_proto = proto.mutable_imu_data();
      // imu的轨迹
      imu_data_proto->set_trajectory_id(trajectory_id);
      // imu数据
      *imu_data_proto->mutable_imu_data() = sensor::ToProto(imu_data);
      writer->WriteProto(proto);
    }
  }
}

写入轮速计数据：

void SerializeOdometryData(
    const sensor::MapByTime<sensor::OdometryData>& all_odometry_data,
    ProtoStreamWriterInterface* const writer) {
  SerializedData proto;
  for (const int trajectory_id : all_odometry_data.trajectory_ids()) {
    // 遍历所有轨迹的轮速计数据
    for (const auto& odometry_data :
         all_odometry_data.trajectory(trajectory_id)) {
      auto* const odometry_data_proto = proto.mutable_odometry_data();
      // 轮速计数据所属的轨迹
      odometry_data_proto->set_trajectory_id(trajectory_id);
      // 轮速计数据
      *odometry_data_proto->mutable_odometry_data() =
          sensor::ToProto(odometry_data);
      writer->WriteProto(proto);
    }
  }
}

读取proto

读取的内容和写入的内容一样，主要的函数接口：

void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader,
                           bool load_frozen_state) {
  io::ProtoStreamDeserializer deserializer(reader);

  // Create a copy of the pose_graph_proto, such that we can re-write the
  // trajectory ids.
  // 读取pose_graph
  proto::PoseGraph pose_graph_proto = deserializer.pose_graph();
  // 读取trajectory参数
  const auto& all_builder_options_proto =
      deserializer.all_trajectory_builder_options();
  // remap trajectory id
  std::map<int, int> trajectory_remapping;
  for (auto& trajectory_proto : *pose_graph_proto.mutable_trajectory()) {
    const auto& options_with_sensor_ids_proto =
        all_builder_options_proto.options_with_sensor_ids(
            trajectory_proto.trajectory_id());
    const int new_trajectory_id =
        AddTrajectoryForDeserialization(options_with_sensor_ids_proto);
    CHECK(trajectory_remapping
              .emplace(trajectory_proto.trajectory_id(), new_trajectory_id)
              .second)
        << "Duplicate trajectory ID: " << trajectory_proto.trajectory_id();
    trajectory_proto.set_trajectory_id(new_trajectory_id);
    if (load_frozen_state) {
      pose_graph_->FreezeTrajectory(new_trajectory_id);
    }
  }

  // Apply the calculated remapping to constraints in the pose graph proto.
  for (auto& constraint_proto : *pose_graph_proto.mutable_constraint()) {
     // remap 约束的trajectory id
    constraint_proto.mutable_submap_id()->set_trajectory_id(
        trajectory_remapping.at(constraint_proto.submap_id().trajectory_id()));
    constraint_proto.mutable_node_id()->set_trajectory_id(
        trajectory_remapping.at(constraint_proto.node_id().trajectory_id()));
  }
  // 分轨迹读取submap的pose
  MapById<SubmapId, transform::Rigid3d> submap_poses;
  for (const proto::Trajectory& trajectory_proto :
       pose_graph_proto.trajectory()) {
    for (const proto::Trajectory::Submap& submap_proto :
         trajectory_proto.submap()) {
      submap_poses.Insert(SubmapId{trajectory_proto.trajectory_id(),
                                   submap_proto.submap_index()},
                          transform::ToRigid3(submap_proto.pose()));
    }
  }
  // 分轨迹读取node的pose
  MapById<NodeId, transform::Rigid3d> node_poses;
  for (const proto::Trajectory& trajectory_proto :
       pose_graph_proto.trajectory()) {
    for (const proto::Trajectory::Node& node_proto : trajectory_proto.node()) {
      node_poses.Insert(
          NodeId{trajectory_proto.trajectory_id(), node_proto.node_index()},
          transform::ToRigid3(node_proto.pose()));
    }
  }

  // Set global poses of landmarks.
  // 读取landmark的pose
  for (const auto& landmark : pose_graph_proto.landmark_poses()) {
    pose_graph_->SetLandmarkPose(landmark.landmark_id(),
                                 transform::ToRigid3(landmark.global_pose()));
  }

  SerializedData proto;
  while (deserializer.ReadNextSerializedData(&proto)) {
    switch (proto.data_case()) {
      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;
      // 读取submap
      case SerializedData::kSubmap: {
        proto.mutable_submap()->mutable_submap_id()->set_trajectory_id(
            trajectory_remapping.at(
                proto.submap().submap_id().trajectory_id()));
        const transform::Rigid3d& submap_pose = submap_poses.at(
            SubmapId{proto.submap().submap_id().trajectory_id(),
                     proto.submap().submap_id().submap_index()});
        pose_graph_->AddSubmapFromProto(submap_pose, proto.submap());
        break;
      }
      // 读取node
      case SerializedData::kNode: {
        proto.mutable_node()->mutable_node_id()->set_trajectory_id(
            trajectory_remapping.at(proto.node().node_id().trajectory_id()));
        const transform::Rigid3d& node_pose =
            node_poses.at(NodeId{proto.node().node_id().trajectory_id(),
                                 proto.node().node_id().node_index()});
        pose_graph_->AddNodeFromProto(node_pose, proto.node());
        break;
      }
      // 读取trajectory data
      case SerializedData::kTrajectoryData: {
        proto.mutable_trajectory_data()->set_trajectory_id(
            trajectory_remapping.at(proto.trajectory_data().trajectory_id()));
        pose_graph_->SetTrajectoryDataFromProto(proto.trajectory_data());
        break;
      }
      // 读取imu数据
      case SerializedData::kImuData: {
        if (load_frozen_state) break;
        pose_graph_->AddImuData(
            trajectory_remapping.at(proto.imu_data().trajectory_id()),
            sensor::FromProto(proto.imu_data().imu_data()));
        break;
      }
      // 读取odometry数据
      case SerializedData::kOdometryData: {
        if (load_frozen_state) break;
        pose_graph_->AddOdometryData(
            trajectory_remapping.at(proto.odometry_data().trajectory_id()),
            sensor::FromProto(proto.odometry_data().odometry_data()));
        break;
      }
      // 读取fix pose数据
      case SerializedData::kFixedFramePoseData: {
        if (load_frozen_state) break;
        pose_graph_->AddFixedFramePoseData(
            trajectory_remapping.at(
                proto.fixed_frame_pose_data().trajectory_id()),
            sensor::FromProto(
                proto.fixed_frame_pose_data().fixed_frame_pose_data()));
        break;
      }
      // 读取landmark数据
      case SerializedData::kLandmarkData: {
        if (load_frozen_state) break;
        pose_graph_->AddLandmarkData(
            trajectory_remapping.at(proto.landmark_data().trajectory_id()),
            sensor::FromProto(proto.landmark_data().landmark_data()));
        break;
      }
      default:
        LOG(WARNING) << "Skipping unknown message type in stream: "
                     << proto.GetTypeName();
    }
  }

  if (load_frozen_state) {
    // Add information about which nodes belong to which submap.
    // Required for 3D pure localization.
    for (const proto::PoseGraph::Constraint& constraint_proto :
         pose_graph_proto.constraint()) {
      if (constraint_proto.tag() !=
          proto::PoseGraph::Constraint::INTRA_SUBMAP) {
        continue;
      }
      pose_graph_->AddNodeToSubmap(
          NodeId{constraint_proto.node_id().trajectory_id(),
                 constraint_proto.node_id().node_index()},
          SubmapId{constraint_proto.submap_id().trajectory_id(),
                   constraint_proto.submap_id().submap_index()});
    }
  } else {
    // When loading unfrozen trajectories, 'AddSerializedConstraints' will
    // take care of adding information about which nodes belong to which
    // submap.
    // 读取约束
    pose_graph_->AddSerializedConstraints(
        FromProto(pose_graph_proto.constraint()));
  }
  CHECK(reader->eof());
}