From e479382ecce87ce4b2dcb6f4429e2f24ed23116f Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 17 Oct 2017 16:03:16 +0200 Subject: [PATCH] Change GetAllSubmapData() to MapById. (#597) --- cartographer/mapping/id.h | 4 +- cartographer/mapping/map_builder.cc | 43 +++++++++--------- .../mapping/proto/sparse_pose_graph.proto | 10 ++--- cartographer/mapping/proto/trajectory.proto | 10 ++++- cartographer/mapping/sparse_pose_graph.cc | 44 ++++++++++--------- cartographer/mapping/sparse_pose_graph.h | 5 ++- cartographer/mapping_2d/sparse_pose_graph.cc | 16 +++---- cartographer/mapping_2d/sparse_pose_graph.h | 2 +- cartographer/mapping_3d/sparse_pose_graph.cc | 16 +++---- cartographer/mapping_3d/sparse_pose_graph.h | 2 +- 10 files changed, 81 insertions(+), 71 deletions(-) diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h index 7b729c5..38fdd80 100644 --- a/cartographer/mapping/id.h +++ b/cartographer/mapping/id.h @@ -243,7 +243,9 @@ class MapById { // Returns an iterator to the element at 'id' or the end iterator if it does // not exist. - ConstIterator find(const IdType& id) { return ConstIterator(*this, id); } + ConstIterator find(const IdType& id) const { + return ConstIterator(*this, id); + } // Inserts data (which must not exist already) into a trajectory. void Insert(const IdType& id, const DataType& data) { diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index daf69d4..a9e31b7 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -140,23 +140,15 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) { writer->WriteProto(sparse_pose_graph_->ToProto()); // Next we serialize all submap data. { - const auto submap_data = sparse_pose_graph_->GetAllSubmapData(); - for (int trajectory_id = 0; - trajectory_id != static_cast(submap_data.size()); - ++trajectory_id) { - for (int submap_index = 0; - submap_index != static_cast(submap_data[trajectory_id].size()); - ++submap_index) { - proto::SerializedData proto; - auto* const submap_proto = proto.mutable_submap(); - // TODO(whess): Handle trimmed data. - submap_proto->mutable_submap_id()->set_trajectory_id(trajectory_id); - submap_proto->mutable_submap_id()->set_submap_index(submap_index); - submap_data[trajectory_id][submap_index].submap->ToProto(submap_proto); - // TODO(whess): Only enable optionally? Resulting pbstream files will be - // a lot larger now. - writer->WriteProto(proto); - } + for (const auto& submap_id_data : sparse_pose_graph_->GetAllSubmapData()) { + proto::SerializedData proto; + auto* const submap_proto = proto.mutable_submap(); + submap_proto->mutable_submap_id()->set_trajectory_id( + submap_id_data.id.trajectory_id); + submap_proto->mutable_submap_id()->set_submap_index( + submap_id_data.id.submap_index); + submap_id_data.data.submap->ToProto(submap_proto); + writer->WriteProto(proto); } } // Next we serialize all node data. @@ -204,6 +196,16 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) { FinishTrajectory(map_trajectory_id); sparse_pose_graph_->FreezeTrajectory(map_trajectory_id); + MapById submap_poses; + for (const proto::Trajectory& trajectory_proto : pose_graph.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())); + } + } + for (;;) { proto::SerializedData proto; if (!reader->ReadProto(&proto)) { @@ -219,10 +221,9 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) { proto.node()); } if (proto.has_submap()) { - const transform::Rigid3d submap_pose = transform::ToRigid3( - pose_graph.trajectory(proto.submap().submap_id().trajectory_id()) - .submap(proto.submap().submap_id().submap_index()) - .pose()); + const transform::Rigid3d submap_pose = + submap_poses.at(SubmapId{proto.submap().submap_id().trajectory_id(), + proto.submap().submap_id().submap_index()}); sparse_pose_graph_->AddSubmapFromProto(map_trajectory_id, submap_pose, proto.submap()); } diff --git a/cartographer/mapping/proto/sparse_pose_graph.proto b/cartographer/mapping/proto/sparse_pose_graph.proto index 04e68b0..588ba83 100644 --- a/cartographer/mapping/proto/sparse_pose_graph.proto +++ b/cartographer/mapping/proto/sparse_pose_graph.proto @@ -31,17 +31,17 @@ message NodeId { message SparsePoseGraph { message Constraint { - // Differentiates between intra-submap (where the scan was inserted into the - // submap) and inter-submap constraints (where the scan was not inserted - // into the submap). + // Differentiates between intra-submap (where the range data was inserted + // into the submap) and inter-submap constraints (where the range data was + // not inserted into the submap). enum Tag { INTRA_SUBMAP = 0; INTER_SUBMAP = 1; } optional SubmapId submap_id = 1; // Submap ID. - optional NodeId node_id = 2; // Scan ID. - // Pose of the scan relative to submap, i.e. taking data from the scan frame + optional NodeId node_id = 2; // Node ID. + // Pose of the node relative to submap, i.e. taking data from the node frame // into the submap frame. optional transform.proto.Rigid3d relative_pose = 3; // Weight of the translational part of the constraint. diff --git a/cartographer/mapping/proto/trajectory.proto b/cartographer/mapping/proto/trajectory.proto index 63858c8..f3d597f 100644 --- a/cartographer/mapping/proto/trajectory.proto +++ b/cartographer/mapping/proto/trajectory.proto @@ -24,14 +24,22 @@ message Trajectory { // NEXT_ID: 7 message Node { optional int64 timestamp = 1; - // Transform from tracking to map frame. + + // Transform from tracking to global map frame. optional transform.proto.Rigid3d pose = 5; } message Submap { + // Index of this submap within its trajectory. + optional int32 submap_index = 2; + + // Transform from submap to global map frame. optional transform.proto.Rigid3d pose = 1; } + // ID of this trajectory. + optional int32 trajectory_id = 3; + // Time-ordered sequence of Nodes. repeated Node node = 1; diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index f37ea30..a324332 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -66,14 +66,23 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( proto::SparsePoseGraph SparsePoseGraph::ToProto() { proto::SparsePoseGraph proto; - std::map node_id_remapping; // Due to trimming. - std::map submap_id_remapping; // Due to trimming. + std::map trajectory_protos; + 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); + }; + + std::map node_id_remapping; // Due to trimming. const auto all_trajectory_nodes = GetTrajectoryNodes(); - const auto all_submap_data = GetAllSubmapData(); for (size_t trajectory_id = 0; trajectory_id != all_trajectory_nodes.size(); ++trajectory_id) { - auto* trajectory_proto = proto.add_trajectory(); + auto* const trajectory_proto = trajectory(trajectory_id); const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id]; for (size_t old_node_index = 0; @@ -90,21 +99,15 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() { *node_proto->mutable_pose() = transform::ToProto(node.global_pose); } } + } - const auto& single_trajectory_submap_data = all_submap_data[trajectory_id]; - for (size_t old_submap_index = 0; - old_submap_index != single_trajectory_submap_data.size(); - ++old_submap_index) { - const auto& submap_data = single_trajectory_submap_data[old_submap_index]; - if (submap_data.submap != nullptr) { - submap_id_remapping[SubmapId{static_cast(trajectory_id), - static_cast(old_submap_index)}] = - SubmapId{static_cast(trajectory_id), - static_cast(trajectory_proto->submap_size())}; - *trajectory_proto->add_submap()->mutable_pose() = - transform::ToProto(submap_data.pose); - } - } + for (const auto& submap_id_data : GetAllSubmapData()) { + CHECK(submap_id_data.data.submap != nullptr); + auto* const submap_proto = + trajectory(submap_id_data.id.trajectory_id)->add_submap(); + submap_proto->set_submap_index(submap_id_data.id.submap_index); + *submap_proto->mutable_pose() = + transform::ToProto(submap_id_data.data.pose); } for (const auto& constraint : constraints()) { @@ -115,11 +118,10 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() { constraint.pose.translation_weight); constraint_proto->set_rotation_weight(constraint.pose.rotation_weight); - const SubmapId submap_id = submap_id_remapping.at(constraint.submap_id); constraint_proto->mutable_submap_id()->set_trajectory_id( - submap_id.trajectory_id); + constraint.submap_id.trajectory_id); constraint_proto->mutable_submap_id()->set_submap_index( - submap_id.submap_index); + constraint.submap_id.submap_index); const NodeId node_id = node_id_remapping.at(constraint.node_id); constraint_proto->mutable_node_id()->set_trajectory_id( diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 73938c8..2a05b1f 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -104,8 +104,9 @@ class SparsePoseGraph { // not exist (anymore). virtual SubmapData GetSubmapData(const SubmapId& submap_id) = 0; - // Returns data for all Submaps by trajectory. - virtual std::vector> GetAllSubmapData() = 0; + // Returns data for all submaps. + virtual mapping::MapById + GetAllSubmapData() = 0; // Returns the transform converting data in the local map frame (i.e. the // continuous, non-loop-closed frame) into the global map frame (i.e. the diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 9ab257e..e435ff1 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -539,18 +539,16 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData( return GetSubmapDataUnderLock(submap_id); } -std::vector> +mapping::MapById SparsePoseGraph::GetAllSubmapData() { common::MutexLocker locker(&mutex_); - std::vector> result; - for (const int trajectory_id : submap_data_.trajectory_ids()) { - result.resize(trajectory_id + 1); - for (const auto& submap_id_data : submap_data_.trajectory(trajectory_id)) { - result[trajectory_id].resize(submap_id_data.id.submap_index + 1); - result[trajectory_id].back() = GetSubmapDataUnderLock(submap_id_data.id); - } + mapping::MapById + submaps; + for (const auto& submap_id_data : submap_data_) { + submaps.Insert(submap_id_data.id, + GetSubmapDataUnderLock(submap_id_data.id)); } - return result; + return submaps; } transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index b322f8c..a108abc 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -94,7 +94,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector> GetConnectedTrajectories() override; mapping::SparsePoseGraph::SubmapData GetSubmapData( const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override; - std::vector> + mapping::MapById GetAllSubmapData() EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 5a1880f..1a4fa46 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -568,18 +568,16 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData( return GetSubmapDataUnderLock(submap_id); } -std::vector> +mapping::MapById SparsePoseGraph::GetAllSubmapData() { common::MutexLocker locker(&mutex_); - std::vector> result; - for (const int trajectory_id : submap_data_.trajectory_ids()) { - result.resize(trajectory_id + 1); - for (const auto& submap_id_data : submap_data_.trajectory(trajectory_id)) { - result[trajectory_id].resize(submap_id_data.id.submap_index + 1); - result[trajectory_id].back() = GetSubmapDataUnderLock(submap_id_data.id); - } + mapping::MapById + submaps; + for (const auto& submap_id_data : submap_data_) { + submaps.Insert(submap_id_data.id, + GetSubmapDataUnderLock(submap_id_data.id)); } - return result; + return submaps; } transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 5218e54..17f37ac 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -94,7 +94,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector> GetConnectedTrajectories() override; mapping::SparsePoseGraph::SubmapData GetSubmapData( const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override; - std::vector> + mapping::MapById GetAllSubmapData() EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override;