Change GetAllSubmapData() to MapById. (#597)
parent
3d4650d675
commit
e479382ecc
|
@ -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) {
|
||||
|
|
|
@ -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<int>(submap_data.size());
|
||||
++trajectory_id) {
|
||||
for (int submap_index = 0;
|
||||
submap_index != static_cast<int>(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<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());
|
||||
}
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -66,14 +66,23 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
|
|||
proto::SparsePoseGraph SparsePoseGraph::ToProto() {
|
||||
proto::SparsePoseGraph proto;
|
||||
|
||||
std::map<NodeId, NodeId> node_id_remapping; // Due to trimming.
|
||||
std::map<SubmapId, SubmapId> submap_id_remapping; // Due to trimming.
|
||||
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.
|
||||
|
||||
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(
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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(
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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(
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue