Add argument to SerializeState for excluding unfinished submaps (#1352)

Adds a boolean argument to MapBuilderInterface::SerializeState() to indicate whether the caller wants to include unfinished submaps in the serialized state.

For cartographer_grpc this argument will be set to false since unfinished submaps do not have a Grid which would lead to a crash in the Submap::ToProto() function.
master
Christoph Schütte 2018-07-30 17:27:54 +02:00 committed by Wally B. Feed
parent 5c61148f26
commit a3c9e9f1ca
20 changed files with 175 additions and 44 deletions

View File

@ -123,7 +123,10 @@ std::string MapBuilderStub::SubmapToProto(
return client.response().error_msg();
}
void MapBuilderStub::SerializeState(io::ProtoStreamWriterInterface* writer) {
void MapBuilderStub::SerializeState(bool include_unfinished_submaps,
io::ProtoStreamWriterInterface* writer) {
CHECK(!include_unfinished_submaps)
<< "Writing of unfinished submaps is unsupported.";
google::protobuf::Empty request;
async_grpc::Client<handlers::WriteStateSignature> client(client_channel_);
CHECK(client.Write(request));

View File

@ -48,7 +48,8 @@ class MapBuilderStub : public mapping::MapBuilderInterface {
std::string SubmapToProto(
const mapping::SubmapId& submap_id,
mapping::proto::SubmapQuery::Response* response) override;
void SerializeState(io::ProtoStreamWriterInterface* writer) override;
void SerializeState(bool include_unfinished_submaps,
io::ProtoStreamWriterInterface* writer) override;
std::map<int, int> LoadState(io::ProtoStreamReaderInterface* reader,
bool load_frozen_state) override;
std::map<int, int> LoadStateFromFile(const std::string& filename) override;

View File

@ -202,7 +202,8 @@ PoseGraphStub::constraints() const {
return mapping::FromProto(client.response().constraints());
}
mapping::proto::PoseGraph PoseGraphStub::ToProto() const {
mapping::proto::PoseGraph PoseGraphStub::ToProto(
bool include_unfinished_submaps) const {
LOG(FATAL) << "Not implemented";
}

View File

@ -52,7 +52,8 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface {
std::map<int, mapping::PoseGraphInterface::TrajectoryData> GetTrajectoryData()
const override;
std::vector<Constraint> constraints() const override;
mapping::proto::PoseGraph ToProto() const override;
mapping::proto::PoseGraph ToProto(
bool include_unfinished_submaps) const override;
void SetGlobalSlamOptimizationCallback(
GlobalSlamOptimizationCallback callback) override;

View File

@ -452,6 +452,25 @@ TEST_F(ClientServerTest, LocalSlam2DWithUploadingServer) {
}
WaitForLocalSlamResults(measurements.size());
WaitForLocalSlamResultUploads(number_of_insertion_results_);
std::queue<std::unique_ptr<google::protobuf::Message>> chunks;
io::ForwardingProtoStreamWriter writer(
[&chunks](const google::protobuf::Message* proto) -> bool {
if (!proto) {
return true;
}
std::unique_ptr<google::protobuf::Message> p(proto->New());
p->CopyFrom(*proto);
chunks.push(std::move(p));
return true;
});
stub_->SerializeState(false, &writer);
CHECK(writer.Close());
// Ensure it can be read.
io::InMemoryProtoStreamReader reader(std::move(chunks));
io::ProtoStreamDeserializer deserializer(&reader);
EXPECT_EQ(deserializer.pose_graph().trajectory_size(), 1);
stub_for_uploading_server_->FinishTrajectory(trajectory_id);
EXPECT_EQ(local_slam_result_poses_.size(), measurements.size());
EXPECT_NEAR(kTravelDistance,

View File

@ -49,7 +49,7 @@ void WriteStateHandler::OnRequest(const google::protobuf::Empty& request) {
return true;
});
GetContext<MapBuilderContextInterface>()->map_builder().SerializeState(
&proto_stream_writer);
/*include_unfinished_submaps=*/false, &proto_stream_writer);
proto_stream_writer.Close();
}

View File

@ -60,9 +60,10 @@ mapping::proto::SerializationHeader CreateHeader() {
return header;
}
SerializedData SerializePoseGraph(const mapping::PoseGraph& pose_graph) {
SerializedData SerializePoseGraph(const mapping::PoseGraph& pose_graph,
bool include_unfinished_submaps) {
SerializedData proto;
*proto.mutable_pose_graph() = pose_graph.ToProto();
*proto.mutable_pose_graph() = pose_graph.ToProto(include_unfinished_submaps);
return proto;
}
@ -79,9 +80,13 @@ SerializedData SerializeTrajectoryBuilderOptions(
void SerializeSubmaps(
const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data,
ProtoStreamWriterInterface* const writer) {
bool include_unfinished_submaps, ProtoStreamWriterInterface* const writer) {
// Next serialize all submaps.
for (const auto& submap_id_data : submap_data) {
if (!include_unfinished_submaps &&
!submap_id_data.data.submap->finished()) {
continue;
}
SerializedData proto;
auto* const submap_proto = proto.mutable_submap();
*submap_proto = submap_id_data.data.submap->ToProto(
@ -209,14 +214,16 @@ void WritePbStream(
const mapping::PoseGraph& pose_graph,
const std::vector<mapping::proto::TrajectoryBuilderOptionsWithSensorIds>&
trajectory_builder_options,
ProtoStreamWriterInterface* const writer) {
ProtoStreamWriterInterface* const writer, bool include_unfinished_submaps) {
writer->WriteProto(CreateHeader());
writer->WriteProto(SerializePoseGraph(pose_graph));
writer->WriteProto(
SerializePoseGraph(pose_graph, include_unfinished_submaps));
writer->WriteProto(SerializeTrajectoryBuilderOptions(
trajectory_builder_options,
GetValidTrajectoryIds(pose_graph.GetTrajectoryStates())));
SerializeSubmaps(pose_graph.GetAllSubmapData(), writer);
SerializeSubmaps(pose_graph.GetAllSubmapData(), include_unfinished_submaps,
writer);
SerializeTrajectoryNodes(pose_graph.GetTrajectoryNodes(), writer);
SerializeTrajectoryData(pose_graph.GetTrajectoryData(), writer);
SerializeImuData(pose_graph.GetImuData(), writer);

View File

@ -32,7 +32,7 @@ void WritePbStream(
const mapping::PoseGraph& pose_graph,
const std::vector<mapping::proto::TrajectoryBuilderOptionsWithSensorIds>&
builder_options,
ProtoStreamWriterInterface* const writer);
ProtoStreamWriterInterface* const writer, bool include_unfinished_submaps);
} // namespace io
} // namespace cartographer

View File

@ -100,7 +100,7 @@ TEST_F(PoseGraph3DTest, Empty) {
EXPECT_TRUE(pose_graph_->GetTrajectoryData().empty());
proto::PoseGraph empty_proto;
EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
pose_graph_->ToProto(), empty_proto));
pose_graph_->ToProto(/*include_unfinished_submaps=*/true), empty_proto));
}
TEST_F(PoseGraph3DTest, BasicSerialization) {
@ -119,7 +119,8 @@ TEST_F(PoseGraph3DTest, BasicSerialization) {
testing::CreateFakeLandmark("landmark_id", Rigid3d::Identity()), &proto);
pose_graph_->SetLandmarkPose("landmark_id", Rigid3d::Identity());
pose_graph_->WaitForAllComputations();
proto::PoseGraph actual_proto = pose_graph_->ToProto();
proto::PoseGraph actual_proto =
pose_graph_->ToProto(/*include_unfinished_submaps=*/true);
EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
proto.constraint(0), actual_proto.constraint(0)));
EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
@ -134,6 +135,59 @@ TEST_F(PoseGraph3DTest, BasicSerialization) {
google::protobuf::util::MessageDifferencer::Equals(proto, actual_proto));
}
TEST_F(PoseGraph3DTest, SerializationWithUnfinishedSubmaps) {
BuildPoseGraph();
proto::PoseGraph proto;
// Create three nodes.
auto fake_node_1 = testing::CreateFakeNode(1, 1);
testing::AddToProtoGraph(fake_node_1, &proto);
pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node_1);
auto fake_node_2 = testing::CreateFakeNode(1, 2);
testing::AddToProtoGraph(fake_node_2, &proto);
pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node_2);
auto fake_node_3 = testing::CreateFakeNode(1, 3);
testing::AddToProtoGraph(fake_node_3, &proto);
pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node_3);
// Create two submaps: one finished, the other not.
auto fake_submap_1 = testing::CreateFakeSubmap3D(1, 1, true);
testing::AddToProtoGraph(fake_submap_1, &proto);
pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), fake_submap_1);
auto fake_submap_2 = testing::CreateFakeSubmap3D(1, 2, false);
testing::AddToProtoGraph(fake_submap_2, &proto);
pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), fake_submap_2);
// Connect node 1 to submap 1, node 2 to submaps 1 and 2, node 3 to submap 2.
testing::AddToProtoGraph(
testing::CreateFakeConstraint(fake_node_1, fake_submap_1), &proto);
testing::AddToProtoGraph(
testing::CreateFakeConstraint(fake_node_2, fake_submap_1), &proto);
testing::AddToProtoGraph(
testing::CreateFakeConstraint(fake_node_2, fake_submap_2), &proto);
testing::AddToProtoGraph(
testing::CreateFakeConstraint(fake_node_3, fake_submap_2), &proto);
pose_graph_->AddSerializedConstraints(FromProto(proto.constraint()));
pose_graph_->WaitForAllComputations();
proto::PoseGraph actual_proto =
pose_graph_->ToProto(/*include_unfinished_submaps=*/false);
EXPECT_EQ(actual_proto.constraint_size(), 2);
EXPECT_EQ(actual_proto.trajectory_size(), 1);
EXPECT_EQ(actual_proto.trajectory(0).node_size(), 2);
EXPECT_EQ(actual_proto.trajectory(0).submap_size(), 1);
EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
proto.constraint(0), actual_proto.constraint(0)));
EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
proto.constraint(1), actual_proto.constraint(1)));
EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
proto.trajectory(0).node(0), actual_proto.trajectory(0).node(0)));
EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
proto.trajectory(0).node(1), actual_proto.trajectory(0).node(1)));
EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
proto.trajectory(0).submap(0), actual_proto.trajectory(0).submap(0)));
}
TEST_F(PoseGraph3DTest, PureLocalizationTrimmer) {
BuildPoseGraphWithFakeOptimization();
const int trajectory_id = 2;

View File

@ -47,7 +47,7 @@ class MockMapBuilder : public mapping::MapBuilderInterface {
MOCK_METHOD2(SubmapToProto,
std::string(const mapping::SubmapId &,
mapping::proto::SubmapQuery::Response *));
MOCK_METHOD1(SerializeState, void(io::ProtoStreamWriterInterface *));
MOCK_METHOD2(SerializeState, void(bool, io::ProtoStreamWriterInterface *));
MOCK_METHOD2(LoadState,
std::map<int, int>(io::ProtoStreamReaderInterface *, bool));
MOCK_METHOD1(LoadStateFromFile, std::map<int, int>(const std::string &));

View File

@ -57,7 +57,7 @@ class MockPoseGraph : public mapping::PoseGraphInterface {
GetTrajectoryData,
std::map<int, mapping::PoseGraphInterface::TrajectoryData>());
MOCK_CONST_METHOD0(constraints, std::vector<Constraint>());
MOCK_CONST_METHOD0(ToProto, mapping::proto::PoseGraph());
MOCK_CONST_METHOD1(ToProto, mapping::proto::PoseGraph(bool));
MOCK_METHOD1(SetGlobalSlamOptimizationCallback,
void(GlobalSlamOptimizationCallback callback));
};

View File

@ -78,13 +78,15 @@ GenerateFakeRangeMeasurements(const Eigen::Vector3f& translation,
return measurements;
}
proto::Submap CreateFakeSubmap3D(int trajectory_id, int submap_index) {
proto::Submap CreateFakeSubmap3D(int trajectory_id, int submap_index,
bool finished) {
proto::Submap proto;
proto.mutable_submap_id()->set_trajectory_id(trajectory_id);
proto.mutable_submap_id()->set_submap_index(submap_index);
proto.mutable_submap_3d()->set_num_range_data(1);
*proto.mutable_submap_3d()->mutable_local_pose() =
transform::ToProto(transform::Rigid3d::Identity());
proto.mutable_submap_3d()->set_finished(finished);
return proto;
}

View File

@ -39,7 +39,8 @@ GenerateFakeRangeMeasurements(const Eigen::Vector3f& translation,
double duration, double time_step,
const transform::Rigid3f& local_to_global);
proto::Submap CreateFakeSubmap3D(int trajectory_id = 1, int submap_index = 1);
proto::Submap CreateFakeSubmap3D(int trajectory_id = 1, int submap_index = 1,
bool finished = true);
proto::Node CreateFakeNode(int trajectory_id = 1, int node_index = 1);

View File

@ -219,8 +219,10 @@ std::string MapBuilder::SubmapToProto(
return "";
}
void MapBuilder::SerializeState(io::ProtoStreamWriterInterface* const writer) {
io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, writer);
void MapBuilder::SerializeState(bool include_unfinished_submaps,
io::ProtoStreamWriterInterface* const writer) {
io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, writer,
include_unfinished_submaps);
}
std::map<int, int> MapBuilder::LoadState(

View File

@ -56,7 +56,8 @@ class MapBuilder : public MapBuilderInterface {
std::string SubmapToProto(const SubmapId &submap_id,
proto::SubmapQuery::Response *response) override;
void SerializeState(io::ProtoStreamWriterInterface *writer) override;
void SerializeState(bool include_unfinished_submaps,
io::ProtoStreamWriterInterface *writer) override;
std::map<int, int> LoadState(io::ProtoStreamReaderInterface *reader,
bool load_frozen_state) override;

View File

@ -77,8 +77,12 @@ class MapBuilderInterface {
virtual std::string SubmapToProto(const SubmapId& submap_id,
proto::SubmapQuery::Response* response) = 0;
// Serializes the current state to a proto stream.
virtual void SerializeState(io::ProtoStreamWriterInterface* writer) = 0;
// Serializes the current state to a proto stream. If
// 'include_unfinished_submaps' is set to true, unfinished submaps, i.e.
// submaps that have not yet received all rangefinder data insertions, will
// be included in the serialized state.
virtual void SerializeState(bool include_unfinished_submaps,
io::ProtoStreamWriterInterface* writer) = 0;
// Loads the SLAM state from a proto stream. Returns the remapping of new
// trajectory_ids.

View File

@ -342,7 +342,7 @@ TEST_F(MapBuilderTest, SaveLoadState) {
// TODO(gaschler): Consider using in-memory to avoid side effects.
const std::string filename = "temp-SaveLoadState.pbstream";
io::ProtoStreamWriter writer(filename);
map_builder_->SerializeState(&writer);
map_builder_->SerializeState(/*include_unfinished_submaps=*/true, &writer);
writer.Close();
// Reset 'map_builder_'.
@ -373,7 +373,7 @@ TEST_F(MapBuilderTest, LocalizationOnFrozenTrajectory2D) {
0);
const std::string filename = "temp-LocalizationOnFrozenTrajectory2D.pbstream";
io::ProtoStreamWriter writer(filename);
map_builder_->SerializeState(&writer);
map_builder_->SerializeState(/*include_unfinished_submaps=*/true, &writer);
writer.Close();
// Reset 'map_builder_'.

View File

@ -115,7 +115,7 @@ proto::PoseGraph::Constraint ToProto(const PoseGraph::Constraint& constraint) {
return constraint_proto;
}
proto::PoseGraph PoseGraph::ToProto() const {
proto::PoseGraph PoseGraph::ToProto(bool include_unfinished_submaps) const {
proto::PoseGraph proto;
std::map<int, proto::Trajectory* const> trajectory_protos;
@ -129,30 +129,62 @@ proto::PoseGraph PoseGraph::ToProto() const {
return trajectory_protos.at(trajectory_id);
};
for (const auto& node_id_data : GetTrajectoryNodes()) {
CHECK(node_id_data.data.constant_data != nullptr);
auto* const node_proto =
trajectory(node_id_data.id.trajectory_id)->add_node();
node_proto->set_node_index(node_id_data.id.node_index);
node_proto->set_timestamp(
common::ToUniversal(node_id_data.data.constant_data->time));
*node_proto->mutable_pose() =
transform::ToProto(node_id_data.data.global_pose);
}
std::set<mapping::SubmapId> unfinished_submaps;
for (const auto& submap_id_data : GetAllSubmapData()) {
proto::Trajectory* trajectory_proto =
trajectory(submap_id_data.id.trajectory_id);
if (!include_unfinished_submaps &&
!submap_id_data.data.submap->finished()) {
// Collect IDs of all unfinished submaps and skip them.
unfinished_submaps.insert(submap_id_data.id);
continue;
}
CHECK(submap_id_data.data.submap != nullptr);
auto* const submap_proto =
trajectory(submap_id_data.id.trajectory_id)->add_submap();
auto* const submap_proto = trajectory_proto->add_submap();
submap_proto->set_submap_index(submap_id_data.id.submap_index);
*submap_proto->mutable_pose() =
transform::ToProto(submap_id_data.data.pose);
}
auto constraints_copy = constraints();
std::set<mapping::NodeId> orphaned_nodes;
proto.mutable_constraint()->Reserve(constraints_copy.size());
for (auto it = constraints_copy.begin(); it != constraints_copy.end();) {
if (!include_unfinished_submaps &&
unfinished_submaps.count(it->submap_id) > 0) {
// Skip all those constraints that refer to unfinished submaps and
// remember the corresponding trajectory nodes as potentially orphaned.
orphaned_nodes.insert(it->node_id);
it = constraints_copy.erase(it);
continue;
}
*proto.add_constraint() = cartographer::mapping::ToProto(*it);
++it;
}
if (!include_unfinished_submaps) {
// Iterate over all constraints and remove trajectory nodes from
// 'orphaned_nodes' that are not actually orphaned.
for (const auto& constraint : constraints_copy) {
*proto.add_constraint() = cartographer::mapping::ToProto(constraint);
orphaned_nodes.erase(constraint.node_id);
}
}
for (const auto& node_id_data : GetTrajectoryNodes()) {
proto::Trajectory* trajectory_proto =
trajectory(node_id_data.id.trajectory_id);
if (!include_unfinished_submaps &&
orphaned_nodes.count(node_id_data.id) > 0) {
// Skip orphaned trajectory nodes.
continue;
}
CHECK(node_id_data.data.constant_data != nullptr);
auto* const node_proto = trajectory_proto->add_node();
node_proto->set_node_index(node_id_data.id.node_index);
node_proto->set_timestamp(
common::ToUniversal(node_id_data.data.constant_data->time));
*node_proto->mutable_pose() =
transform::ToProto(node_id_data.data.global_pose);
}
auto landmarks_copy = GetLandmarkPoses();

View File

@ -117,7 +117,7 @@ class PoseGraph : public PoseGraphInterface {
// not exist (anymore).
virtual SubmapData GetSubmapData(const SubmapId& submap_id) const = 0;
proto::PoseGraph ToProto() const override;
proto::PoseGraph ToProto(bool include_unfinished_submaps) const override;
// Returns the IMU data.
virtual sensor::MapByTime<sensor::ImuData> GetImuData() const = 0;

View File

@ -140,8 +140,11 @@ class PoseGraphInterface {
// Returns the collection of constraints.
virtual std::vector<Constraint> constraints() const = 0;
// Serializes the constraints and trajectories.
virtual proto::PoseGraph ToProto() const = 0;
// Serializes the constraints and trajectories. If
// 'include_unfinished_submaps' is set to 'true', unfinished submps, i.e.
// submaps that have not yet received all rangefinder data insertions, will
// be included, otherwise not.
virtual proto::PoseGraph ToProto(bool include_unfinished_submaps) const = 0;
// Sets the callback function that is invoked whenever the global optimization
// problem is solved.