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

Return only optimized & finished submaps for trimmer. (#1104)

master
Alexander Belyaev 2018-04-20 22:19:16 +02:00 committed by Christoph Schütte
parent 4aa2af23de
commit e2623991da
7 changed files with 35 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc
View File

@ -178,7 +178,7 @@ std::vector<SubmapId> FindSubmapIdsToTrim(
} // namespace } // namespace
void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) { void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) {
const auto submap_data = pose_graph->GetAllSubmapData(); const auto submap_data = pose_graph->GetOptimizedSubmapData();
if (submap_data.size() - current_submap_count_ <= min_added_submaps_count_) { if (submap_data.size() - current_submap_count_ <= min_added_submaps_count_) {
return; return;
} }

16
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -797,8 +797,20 @@ int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const {
} }
MapById<SubmapId, PoseGraphInterface::SubmapData> MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph2D::TrimmingHandle::GetAllSubmapData() const { PoseGraph2D::TrimmingHandle::GetOptimizedSubmapData() const {
return parent_->GetSubmapDataUnderLock(); MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
for (const auto& submap_id_data : parent_->submap_data_) {
if (submap_id_data.data.state != SubmapState::kFinished ||
!parent_->global_submap_poses_.Contains(submap_id_data.id)) {
continue;
}
submaps.Insert(submap_id_data.id,
SubmapData{submap_id_data.data.submap,
transform::Embed3D(parent_->global_submap_poses_
.at(submap_id_data.id)
.global_pose)});
}
return submaps;
} }
std::vector<SubmapId> PoseGraph2D::TrimmingHandle::GetSubmapIds( std::vector<SubmapId> PoseGraph2D::TrimmingHandle::GetSubmapIds(

2
cartographer/mapping/internal/2d/pose_graph_2d.h
View File

@ -281,7 +281,7 @@ class PoseGraph2D : public PoseGraph {
int num_submaps(int trajectory_id) const override; int num_submaps(int trajectory_id) const override;
std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override; std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
MapById<SubmapId, SubmapData> GetAllSubmapData() const override MapById<SubmapId, SubmapData> GetOptimizedSubmapData() const override
REQUIRES(parent_->mutex_); REQUIRES(parent_->mutex_);
const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
REQUIRES(parent_->mutex_); REQUIRES(parent_->mutex_);

16
cartographer/mapping/internal/3d/pose_graph_3d.cc
View File

@ -835,8 +835,20 @@ std::vector<SubmapId> PoseGraph3D::TrimmingHandle::GetSubmapIds(
return submap_ids; return submap_ids;
} }
MapById<SubmapId, PoseGraphInterface::SubmapData> MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph3D::TrimmingHandle::GetAllSubmapData() const { PoseGraph3D::TrimmingHandle::GetOptimizedSubmapData() const {
return parent_->GetSubmapDataUnderLock(); MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
for (const auto& submap_id_data : parent_->submap_data_) {
if (submap_id_data.data.state != SubmapState::kFinished ||
!parent_->global_submap_poses_.Contains(submap_id_data.id)) {
continue;
}
submaps.Insert(
submap_id_data.id,
SubmapData{
submap_id_data.data.submap,
parent_->global_submap_poses_.at(submap_id_data.id).global_pose});
}
return submaps;
} }
const MapById<NodeId, TrajectoryNode>& const MapById<NodeId, TrajectoryNode>&

2
cartographer/mapping/internal/3d/pose_graph_3d.h
View File

@ -285,7 +285,7 @@ class PoseGraph3D : public PoseGraph {
int num_submaps(int trajectory_id) const override; int num_submaps(int trajectory_id) const override;
std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override; std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
MapById<SubmapId, SubmapData> GetAllSubmapData() const override MapById<SubmapId, SubmapData> GetOptimizedSubmapData() const override
REQUIRES(parent_->mutex_); REQUIRES(parent_->mutex_);
const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
REQUIRES(parent_->mutex_); REQUIRES(parent_->mutex_);

2
cartographer/mapping/internal/testing/fake_trimmable.h
View File

@ -58,7 +58,7 @@ class FakeTrimmable : public Trimmable {
return &submap_data_; return &submap_data_;
} }
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() MapById<SubmapId, PoseGraphInterface::SubmapData> GetOptimizedSubmapData()
const override { const override {
return submap_data_; return submap_data_;
} }

5
cartographer/mapping/pose_graph_trimmer.h
View File

@ -32,8 +32,9 @@ class Trimmable {
virtual int num_submaps(int trajectory_id) const = 0; virtual int num_submaps(int trajectory_id) const = 0;
virtual std::vector<SubmapId> GetSubmapIds(int trajectory_id) const = 0; virtual std::vector<SubmapId> GetSubmapIds(int trajectory_id) const = 0;
virtual MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() // Returns finished submaps with optimized poses only.
const = 0; virtual MapById<SubmapId, PoseGraphInterface::SubmapData>
GetOptimizedSubmapData() const = 0;
virtual const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const = 0; virtual const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const = 0;
virtual const std::vector<PoseGraphInterface::Constraint>& GetConstraints() virtual const std::vector<PoseGraphInterface::Constraint>& GetConstraints()
const = 0; const = 0;