diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 868ee45..185d3f6 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -635,6 +635,12 @@ sensor::MapByTime PoseGraph2D::GetOdometryData() { return optimization_problem_->odometry_data(); } +std::map +PoseGraph2D::GetLandmarkNodes() { + common::MutexLocker locker(&mutex_); + return landmark_nodes_; +} + std::map PoseGraph2D::GetTrajectoryData() { return {}; // Not implemented yet in 2D. diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index 2ea8b00..c0e4c80 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -126,6 +126,8 @@ class PoseGraph2D : public PoseGraph { EXCLUDES(mutex_); sensor::MapByTime GetFixedFramePoseData() override EXCLUDES(mutex_); + std::map + GetLandmarkNodes() override EXCLUDES(mutex_); std::map GetTrajectoryData() override EXCLUDES(mutex_); std::vector constraints() override EXCLUDES(mutex_); void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id, diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 7d87bea..ae1463f 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -684,6 +684,12 @@ PoseGraph3D::GetFixedFramePoseData() { return optimization_problem_->fixed_frame_pose_data(); } +std::map +PoseGraph3D::GetLandmarkNodes() { + common::MutexLocker locker(&mutex_); + return landmark_nodes_; +} + std::map PoseGraph3D::GetTrajectoryData() { common::MutexLocker locker(&mutex_); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index 0f21c6f..01ea013 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -125,6 +125,8 @@ class PoseGraph3D : public PoseGraph { EXCLUDES(mutex_); sensor::MapByTime GetFixedFramePoseData() override EXCLUDES(mutex_); + std::map + GetLandmarkNodes() override EXCLUDES(mutex_); std::map GetTrajectoryData() override; std::vector constraints() override EXCLUDES(mutex_); diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 669afc3..8919f11 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -286,7 +286,29 @@ void MapBuilder::SerializeState(io::ProtoStreamWriterInterface* const writer) { writer->WriteProto(proto); } } - // TODO(pifon2a, ojura): serialize landmarks + // Next we serialize all landmark data. + { + const std::map + all_landmark_nodes = pose_graph_->GetLandmarkNodes(); + for (const auto& node : all_landmark_nodes) { + for (const auto& observation : node.second.landmark_observations) { + proto::SerializedData proto; + auto* landmark_data_proto = proto.mutable_landmark_data(); + landmark_data_proto->set_trajectory_id(observation.trajectory_id); + landmark_data_proto->mutable_landmark_data()->set_timestamp( + common::ToUniversal(observation.time)); + auto* observation_proto = landmark_data_proto->mutable_landmark_data() + ->add_landmark_observations(); + observation_proto->set_id(node.first); + *observation_proto->mutable_landmark_to_tracking_transform() = + transform::ToProto(observation.landmark_to_tracking_transform); + observation_proto->set_translation_weight( + observation.translation_weight); + observation_proto->set_rotation_weight(observation.rotation_weight); + writer->WriteProto(proto); + } + } + } } void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader, diff --git a/cartographer/mapping/pose_graph.h b/cartographer/mapping/pose_graph.h index abfdfcd..ffec3aa 100644 --- a/cartographer/mapping/pose_graph.h +++ b/cartographer/mapping/pose_graph.h @@ -129,6 +129,10 @@ class PoseGraph : public PoseGraphInterface { virtual sensor::MapByTime GetFixedFramePoseData() = 0; + // Returns the landmark data. + virtual std::map + GetLandmarkNodes() = 0; + // Sets a relative initial pose 'relative_pose' for 'from_trajectory_id' with // respect to 'to_trajectory_id' at time 'time'. virtual void SetInitialTrajectoryPose(int from_trajectory_id,