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

Introduce PoseGraphInterface::GetAllSubmapPoses() (#790)

master
Christoph Schütte 2018-01-05 14:19:08 +01:00 committed by Wally B. Feed
parent 63a80c9340
commit 1a837ef3ab
7 changed files with 57 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cartographer/mapping/pose_graph_interface.h
View File

@ -49,6 +49,11 @@ class PoseGraphInterface {
enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
};
struct SubmapPose {
int version;
transform::Rigid3d pose;
};
struct SubmapData {
std::shared_ptr<const Submap> submap;
transform::Rigid3d pose;
@ -66,6 +71,9 @@ class PoseGraphInterface {
// Returns data for all submaps.
virtual MapById<SubmapId, SubmapData> GetAllSubmapData() = 0;
// Returns the global poses for all submaps.
virtual MapById<SubmapId, SubmapPose> GetAllSubmapPoses() = 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
// discontinuous, loop-closed frame).

19
cartographer/mapping_2d/pose_graph.cc
View File

@ -652,10 +652,11 @@ mapping::PoseGraph::SubmapData PoseGraph::GetSubmapData(
return GetSubmapDataUnderLock(submap_id);
}
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData>
PoseGraph::GetAllSubmapData() {
common::MutexLocker locker(&mutex_);
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData> submaps;
mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData>
submaps;
for (const auto& submap_id_data : submap_data_) {
submaps.Insert(submap_id_data.id,
GetSubmapDataUnderLock(submap_id_data.id));
@ -663,6 +664,20 @@ PoseGraph::GetAllSubmapData() {
return submaps;
}
mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapPose>
PoseGraph::GetAllSubmapPoses() {
common::MutexLocker locker(&mutex_);
mapping::MapById<mapping::SubmapId, SubmapPose> submap_poses;
for (const auto& submap_id_data : submap_data_) {
auto submap_data = GetSubmapDataUnderLock(submap_id_data.id);
submap_poses.Insert(
submap_id_data.id,
mapping::PoseGraph::SubmapPose{submap_data.submap->num_range_data(),
submap_data.pose});
}
return submap_poses;
}
transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform(
const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>&
global_submap_poses,

4
cartographer/mapping_2d/pose_graph.h
View File

@ -102,8 +102,10 @@ class PoseGraph : public mapping::PoseGraph {
std::vector<std::vector<int>> GetConnectedTrajectories() override;
mapping::PoseGraph::SubmapData GetSubmapData(
const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData>
GetAllSubmapData() EXCLUDES(mutex_) override;
mapping::MapById<mapping::SubmapId, SubmapPose> GetAllSubmapPoses()
EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override;
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>

19
cartographer/mapping_3d/pose_graph.cc
View File

@ -671,10 +671,11 @@ mapping::PoseGraph::SubmapData PoseGraph::GetSubmapData(
return GetSubmapDataUnderLock(submap_id);
}
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData>
PoseGraph::GetAllSubmapData() {
common::MutexLocker locker(&mutex_);
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData> submaps;
mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData>
submaps;
for (const auto& submap_id_data : submap_data_) {
submaps.Insert(submap_id_data.id,
GetSubmapDataUnderLock(submap_id_data.id));
@ -682,6 +683,20 @@ PoseGraph::GetAllSubmapData() {
return submaps;
}
mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapPose>
PoseGraph::GetAllSubmapPoses() {
common::MutexLocker locker(&mutex_);
mapping::MapById<mapping::SubmapId, SubmapPose> submap_poses;
for (const auto& submap_id_data : submap_data_) {
auto submap_data = GetSubmapDataUnderLock(submap_id_data.id);
submap_poses.Insert(
submap_id_data.id,
mapping::PoseGraph::SubmapPose{submap_data.submap->num_range_data(),
submap_data.pose});
}
return submap_poses;
}
transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform(
const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>&
global_submap_poses,

4
cartographer/mapping_3d/pose_graph.h
View File

@ -102,8 +102,10 @@ class PoseGraph : public mapping::PoseGraph {
std::vector<std::vector<int>> GetConnectedTrajectories() override;
mapping::PoseGraph::SubmapData GetSubmapData(
const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData>
GetAllSubmapData() EXCLUDES(mutex_) override;
mapping::MapById<mapping::SubmapId, SubmapPose> GetAllSubmapPoses()
EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override;
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>

7
cartographer_grpc/mapping/pose_graph_stub.cc
View File

@ -34,6 +34,13 @@ PoseGraphStub::GetAllSubmapData() {
LOG(FATAL) << "Not implemented";
}
cartographer::mapping::MapById<
cartographer::mapping::SubmapId,
cartographer::mapping::PoseGraphInterface::SubmapPose>
PoseGraphStub::GetAllSubmapPoses() {
LOG(FATAL) << "Not implemented";
}
cartographer::transform::Rigid3d PoseGraphStub::GetLocalToGlobalTransform(
int trajectory_id) {
LOG(FATAL) << "Not implemented";

2
cartographer_grpc/mapping/pose_graph_stub.h
View File

@ -35,6 +35,8 @@ class PoseGraphStub : public cartographer::mapping::PoseGraphInterface {
void RunFinalOptimization() override;
cartographer::mapping::MapById<cartographer::mapping::SubmapId, SubmapData>
GetAllSubmapData() override;
cartographer::mapping::MapById<cartographer::mapping::SubmapId, SubmapPose>
GetAllSubmapPoses() override;
cartographer::transform::Rigid3d GetLocalToGlobalTransform(
int trajectory_id) override;
cartographer::mapping::MapById<cartographer::mapping::NodeId,