From 2bd987ffb40e2d6667682c5c96432173f371094f Mon Sep 17 00:00:00 2001 From: Sebastian Klose Date: Tue, 26 Jun 2018 13:36:42 +0200 Subject: [PATCH] Fix serialization for deleted trajectories. (#1214) * update TrajectoryState of trajectories that got 'trimmed' away to be deleted in the PureLocalizationTrimmer * update serialization to only serialize 'undeleted' trajectories and corresponding options. #1111 --- .../internal/mapping_state_serialization.cc | 36 +++++++++++++------ .../mapping/internal/2d/pose_graph_2d.cc | 5 +++ .../mapping/internal/2d/pose_graph_2d.h | 2 ++ .../mapping/internal/3d/pose_graph_3d.cc | 5 +++ .../mapping/internal/3d/pose_graph_3d.h | 3 ++ .../mapping/internal/testing/fake_trimmable.h | 4 +++ cartographer/mapping/pose_graph_trimmer.cc | 2 ++ cartographer/mapping/pose_graph_trimmer.h | 4 +++ 8 files changed, 51 insertions(+), 10 deletions(-) diff --git a/cartographer/io/internal/mapping_state_serialization.cc b/cartographer/io/internal/mapping_state_serialization.cc index 51bf16d..5b6208a 100644 --- a/cartographer/io/internal/mapping_state_serialization.cc +++ b/cartographer/io/internal/mapping_state_serialization.cc @@ -31,16 +31,29 @@ using mapping::proto::SerializedData; mapping::proto::AllTrajectoryBuilderOptions CreateAllTrajectoryBuilderOptionsProto( const std::vector& - all_options_with_sensor_ids) { + all_options_with_sensor_ids, + const std::vector& trajectory_ids_to_serialize) { 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; + for (auto id : trajectory_ids_to_serialize) { + *all_options_proto.add_options_with_sensor_ids() = + all_options_with_sensor_ids[id]; } - CHECK_EQ(all_options_with_sensor_ids.size(), - all_options_proto.options_with_sensor_ids_size()); return all_options_proto; } +// Will return all trajectory ids, that have `state != DELETED`. +std::vector GetValidTrajectoryIds( + const std::map& + trajectory_states) { + std::vector valid_trajectories; + for (const auto& t : trajectory_states) { + if (t.second != PoseGraphInterface::TrajectoryState::DELETED) { + valid_trajectories.push_back(t.first); + } + } + return valid_trajectories; +} + mapping::proto::SerializationHeader CreateHeader() { mapping::proto::SerializationHeader header; header.set_format_version(kMappingStateSerializationFormatVersion); @@ -53,12 +66,14 @@ SerializedData SerializePoseGraph(const mapping::PoseGraph& pose_graph) { return proto; } -SerializedData SerializeAllTrajectoryBuilderOptions( +SerializedData SerializeTrajectoryBuilderOptions( const std::vector& - trajectory_builder_options) { + trajectory_builder_options, + const std::vector& trajectory_ids_to_serialize) { SerializedData proto; *proto.mutable_all_trajectory_builder_options() = - CreateAllTrajectoryBuilderOptionsProto(trajectory_builder_options); + CreateAllTrajectoryBuilderOptionsProto(trajectory_builder_options, + trajectory_ids_to_serialize); return proto; } @@ -197,8 +212,9 @@ void WritePbStream( ProtoStreamWriterInterface* const writer) { writer->WriteProto(CreateHeader()); writer->WriteProto(SerializePoseGraph(pose_graph)); - writer->WriteProto( - SerializeAllTrajectoryBuilderOptions(trajectory_builder_options)); + writer->WriteProto(SerializeTrajectoryBuilderOptions( + trajectory_builder_options, + GetValidTrajectoryIds(pose_graph.GetTrajectoryStates()))); SerializeSubmaps(pose_graph.GetAllSubmapData(), writer); SerializeTrajectoryNodes(pose_graph.GetTrajectoryNodes(), writer); diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 65e9195..505805d 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -986,6 +986,11 @@ bool PoseGraph2D::TrimmingHandle::IsFinished(const int trajectory_id) const { return parent_->IsTrajectoryFinished(trajectory_id); } +void PoseGraph2D::TrimmingHandle::SetTrajectoryState(int trajectory_id, + TrajectoryState state) { + parent_->data_.trajectories_state[trajectory_id].state = state; +} + void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) { // TODO(hrapp): We have to make sure that the trajectory has been finished // if we want to delete the last submaps. diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index b81770b..6a1032e 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -264,6 +264,8 @@ class PoseGraph2D : public PoseGraph { void TrimSubmap(const SubmapId& submap_id) REQUIRES(parent_->mutex_) override; bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_); + void SetTrajectoryState(int trajectory_id, TrajectoryState state) override + REQUIRES(parent_->mutex_); private: PoseGraph2D* const parent_; diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 31bfca7..8d15378 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -1012,6 +1012,11 @@ bool PoseGraph3D::TrimmingHandle::IsFinished(const int trajectory_id) const { return parent_->IsTrajectoryFinished(trajectory_id); } +void PoseGraph3D::TrimmingHandle::SetTrajectoryState(int trajectory_id, + TrajectoryState state) { + parent_->data_.trajectories_state[trajectory_id].state = state; +} + void PoseGraph3D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) { // TODO(hrapp): We have to make sure that the trajectory has been finished // if we want to delete the last submaps. diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index 51b396e..5e35319 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -268,6 +268,9 @@ class PoseGraph3D : public PoseGraph { REQUIRES(parent_->mutex_) override; bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_); + void SetTrajectoryState(int trajectory_id, TrajectoryState state) override + REQUIRES(parent_->mutex_); + private: PoseGraph3D* const parent_; }; diff --git a/cartographer/mapping/internal/testing/fake_trimmable.h b/cartographer/mapping/internal/testing/fake_trimmable.h index 8821cff..aeb80f9 100644 --- a/cartographer/mapping/internal/testing/fake_trimmable.h +++ b/cartographer/mapping/internal/testing/fake_trimmable.h @@ -96,6 +96,10 @@ class FakeTrimmable : public Trimmable { bool IsFinished(const int trajectory_id) const override { return false; } + void SetTrajectoryState( + int /*trajectory_id*/, + PoseGraphInterface::TrajectoryState /*state*/) override {} + std::vector trimmed_submaps() { return trimmed_submaps_; } private: diff --git a/cartographer/mapping/pose_graph_trimmer.cc b/cartographer/mapping/pose_graph_trimmer.cc index d6ba14e..27af3f9 100644 --- a/cartographer/mapping/pose_graph_trimmer.cc +++ b/cartographer/mapping/pose_graph_trimmer.cc @@ -39,6 +39,8 @@ void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) { if (num_submaps_to_keep_ == 0) { finished_ = true; + pose_graph->SetTrajectoryState( + trajectory_id_, PoseGraphInterface::TrajectoryState::DELETED); } } diff --git a/cartographer/mapping/pose_graph_trimmer.h b/cartographer/mapping/pose_graph_trimmer.h index 6046a99..87c2cc8 100644 --- a/cartographer/mapping/pose_graph_trimmer.h +++ b/cartographer/mapping/pose_graph_trimmer.h @@ -46,6 +46,10 @@ class Trimmable { // Checks if the given trajectory is finished or not. virtual bool IsFinished(int trajectory_id) const = 0; + + // Sets the state for a specific trajectory. + virtual void SetTrajectoryState( + int trajectory_id, PoseGraphInterface::TrajectoryState state) = 0; }; // An interface to implement algorithms that choose how to trim the pose graph.