12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Change GetAllSubmapData() to MapById. (#597)

master
Wolfgang Hess 2017-10-17 16:03:16 +02:00 committed by GitHub
parent 3d4650d675
commit e479382ecc
10 changed files with 81 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/mapping/id.h
View File

@ -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) {

37
cartographer/mapping/map_builder.cc
View File

@ -140,25 +140,17 @@ 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<int>(submap_data.size());
++trajectory_id) {
for (int submap_index = 0;
submap_index != static_cast<int>(submap_data[trajectory_id].size());
++submap_index) {
for (const auto& submap_id_data : sparse_pose_graph_->GetAllSubmapData()) {
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.
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.
{
const auto trajectory_nodes = sparse_pose_graph_->GetTrajectoryNodes();
@ -204,6 +196,16 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
FinishTrajectory(map_trajectory_id);
sparse_pose_graph_->FreezeTrajectory(map_trajectory_id);
MapById<SubmapId, transform::Rigid3d> 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());
}

10
cartographer/mapping/proto/sparse_pose_graph.proto
View File

@ -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.

10
cartographer/mapping/proto/trajectory.proto
View File

@ -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;

42
cartographer/mapping/sparse_pose_graph.cc
View File

@ -66,14 +66,23 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
proto::SparsePoseGraph SparsePoseGraph::ToProto() {
proto::SparsePoseGraph proto;
std::map<int, proto::Trajectory* const> 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<NodeId, NodeId> node_id_remapping; // Due to trimming.
std::map<SubmapId, SubmapId> submap_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<int>(trajectory_id),
static_cast<int>(old_submap_index)}] =
SubmapId{static_cast<int>(trajectory_id),
static_cast<int>(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(

5
cartographer/mapping/sparse_pose_graph.h
View File

@ -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<std::vector<SubmapData>> GetAllSubmapData() = 0;
// Returns data for all submaps.
virtual mapping::MapById<mapping::SubmapId, SubmapData>
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

16
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -539,18 +539,16 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
return GetSubmapDataUnderLock(submap_id);
}
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData>
SparsePoseGraph::GetAllSubmapData() {
common::MutexLocker locker(&mutex_);
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>> 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<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData>
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(

2
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -94,7 +94,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::vector<std::vector<int>> GetConnectedTrajectories() override;
mapping::SparsePoseGraph::SubmapData GetSubmapData(
const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData>
GetAllSubmapData() EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override;

16
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -568,18 +568,16 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
return GetSubmapDataUnderLock(submap_id);
}
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData>
SparsePoseGraph::GetAllSubmapData() {
common::MutexLocker locker(&mutex_);
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>> 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<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData>
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(

2
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -94,7 +94,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::vector<std::vector<int>> GetConnectedTrajectories() override;
mapping::SparsePoseGraph::SubmapData GetSubmapData(
const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData>
GetAllSubmapData() EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override;