Cartographer源码阅读2D&3D-Pbstream读写
写入proto
查看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());
}