diff --git a/cartographer/cloud/client/map_builder_stub.cc b/cartographer/cloud/client/map_builder_stub.cc index 0690cc1..c326223 100644 --- a/cartographer/cloud/client/map_builder_stub.cc +++ b/cartographer/cloud/client/map_builder_stub.cc @@ -124,8 +124,8 @@ void MapBuilderStub::SerializeState(io::ProtoStreamWriterInterface* writer) { } } -void MapBuilderStub::LoadState(io::ProtoStreamReaderInterface* reader, - const bool load_frozen_state) { +std::map MapBuilderStub::LoadState( + io::ProtoStreamReaderInterface* reader, const bool load_frozen_state) { if (!load_frozen_state) { LOG(FATAL) << "Not implemented"; } @@ -163,6 +163,13 @@ void MapBuilderStub::LoadState(io::ProtoStreamReaderInterface* reader, CHECK(reader->eof()); CHECK(client.StreamWritesDone()); CHECK(client.StreamFinish().ok()); + // TODO(gaschler): Return trajectory remapping. + return {}; +} + +std::map MapBuilderStub::LoadStateFromFile( + const std::string& filename) { + LOG(FATAL) << "not implemented"; } int MapBuilderStub::num_trajectory_builders() const { diff --git a/cartographer/cloud/client/map_builder_stub.h b/cartographer/cloud/client/map_builder_stub.h index d102cef..c60a4a8 100644 --- a/cartographer/cloud/client/map_builder_stub.h +++ b/cartographer/cloud/client/map_builder_stub.h @@ -48,8 +48,9 @@ class MapBuilderStub : public mapping::MapBuilderInterface { const mapping::SubmapId& submap_id, mapping::proto::SubmapQuery::Response* response) override; void SerializeState(io::ProtoStreamWriterInterface* writer) override; - void LoadState(io::ProtoStreamReaderInterface* reader, - bool load_frozen_state) override; + std::map LoadState(io::ProtoStreamReaderInterface* reader, + bool load_frozen_state) override; + std::map LoadStateFromFile(const std::string& filename) override; int num_trajectory_builders() const override; mapping::PoseGraphInterface* pose_graph() override; const std::vector& diff --git a/cartographer/cloud/internal/client_server_test.cc b/cartographer/cloud/internal/client_server_test.cc index 0fcf188..bf7e572 100644 --- a/cartographer/cloud/internal/client_server_test.cc +++ b/cartographer/cloud/internal/client_server_test.cc @@ -448,6 +448,8 @@ TEST_F(ClientServerTest, LoadState) { server_->Shutdown(); } +// TODO(gaschler): Test-cover LoadStateFromFile. + } // namespace } // namespace cloud } // namespace cartographer diff --git a/cartographer/mapping/internal/testing/mock_map_builder.h b/cartographer/mapping/internal/testing/mock_map_builder.h index 78e8af9..23b1fb4 100644 --- a/cartographer/mapping/internal/testing/mock_map_builder.h +++ b/cartographer/mapping/internal/testing/mock_map_builder.h @@ -48,7 +48,9 @@ class MockMapBuilder : public mapping::MapBuilderInterface { std::string(const mapping::SubmapId &, mapping::proto::SubmapQuery::Response *)); MOCK_METHOD1(SerializeState, void(io::ProtoStreamWriterInterface *)); - MOCK_METHOD2(LoadState, void(io::ProtoStreamReaderInterface *, bool)); + MOCK_METHOD2(LoadState, + std::map(io::ProtoStreamReaderInterface *, bool)); + MOCK_METHOD1(LoadStateFromFile, std::map(const std::string &)); MOCK_CONST_METHOD0(num_trajectory_builders, int()); MOCK_METHOD0(pose_graph, mapping::PoseGraphInterface *()); MOCK_CONST_METHOD0( diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 3ad6de5..d0ba2df 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -19,6 +19,7 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/time.h" #include "cartographer/io/internal/mapping_state_serialization.h" +#include "cartographer/io/proto_stream.h" #include "cartographer/io/proto_stream_deserializer.h" #include "cartographer/mapping/internal/2d/local_trajectory_builder_2d.h" #include "cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h" @@ -206,8 +207,8 @@ void MapBuilder::SerializeState(io::ProtoStreamWriterInterface* const writer) { io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, writer); } -void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader, - bool load_frozen_state) { +std::map 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 @@ -364,6 +365,20 @@ void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader, FromProto(pose_graph_proto.constraint())); } CHECK(reader->eof()); + return trajectory_remapping; +} + +std::map MapBuilder::LoadStateFromFile( + const std::string& state_filename) { + const std::string suffix = ".pbstream"; + if (state_filename.substr( + std::max(state_filename.size() - suffix.size(), 0)) != suffix) { + LOG(WARNING) << "The file containing the state should be a " + ".pbstream file."; + } + LOG(INFO) << "Loading saved state '" << state_filename << "'..."; + io::ProtoStreamReader stream(state_filename); + return LoadState(&stream, true); } } // namespace mapping diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index 880f3bd..8d3f847 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -58,8 +58,10 @@ class MapBuilder : public MapBuilderInterface { void SerializeState(io::ProtoStreamWriterInterface *writer) override; - void LoadState(io::ProtoStreamReaderInterface *reader, - bool load_frozen_state) override; + std::map LoadState(io::ProtoStreamReaderInterface *reader, + bool load_frozen_state) override; + + std::map LoadStateFromFile(const std::string &filename) override; mapping::PoseGraphInterface *pose_graph() override { return pose_graph_.get(); diff --git a/cartographer/mapping/map_builder_interface.h b/cartographer/mapping/map_builder_interface.h index 5b0cbe3..fd57fe2 100644 --- a/cartographer/mapping/map_builder_interface.h +++ b/cartographer/mapping/map_builder_interface.h @@ -80,9 +80,15 @@ class MapBuilderInterface { // Serializes the current state to a proto stream. virtual void SerializeState(io::ProtoStreamWriterInterface* writer) = 0; - // Loads the SLAM state from a proto stream. - virtual void LoadState(io::ProtoStreamReaderInterface* reader, - bool load_frozen_state) = 0; + // Loads the SLAM state from a proto stream. Returns the remapping of new + // trajectory_ids. + virtual std::map + LoadState(io::ProtoStreamReaderInterface* reader, bool load_frozen_state) = 0; + + // Loads the SLAM state froma a pbstream file. Returns the remapping of new + // trajectory_ids. + virtual std::map + LoadStateFromFile(const std::string& filename) = 0; virtual int num_trajectory_builders() const = 0;