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

Use references for constraints & nodes in TrimmingHandle. (#1091)

master
Alexander Belyaev 2018-04-19 17:09:16 +02:00 committed by Wally B. Feed
parent 20c80068b2
commit ab7402ffbb
7 changed files with 19 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -181,14 +181,12 @@ void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) {
const auto submap_data = pose_graph->GetAllSubmapData(); const auto submap_data = pose_graph->GetAllSubmapData();
if (submap_data.size() == 0) return; if (submap_data.size() == 0) return;
const auto constraints = pose_graph->GetConstraints();
const auto trajectory_nodes = pose_graph->GetTrajectoryNodes();
SubmapCoverageGrid2D coverage_grid(GetCornerOfFirstSubmap(submap_data)); SubmapCoverageGrid2D coverage_grid(GetCornerOfFirstSubmap(submap_data));
const std::map<SubmapId, common::Time> submap_freshness =
ComputeSubmapFreshness(submap_data, pose_graph->GetTrajectoryNodes(),
pose_graph->GetConstraints());
const std::set<SubmapId> all_submap_ids = AddSubmapsToSubmapCoverageGrid2D( const std::set<SubmapId> all_submap_ids = AddSubmapsToSubmapCoverageGrid2D(
ComputeSubmapFreshness(submap_data, trajectory_nodes, constraints), submap_freshness, submap_data, &coverage_grid);
submap_data, &coverage_grid);
const std::vector<SubmapId> submap_ids_to_remove = const std::vector<SubmapId> submap_ids_to_remove =
FindSubmapIdsToTrim(coverage_grid, all_submap_ids, fresh_submaps_count_, FindSubmapIdsToTrim(coverage_grid, all_submap_ids, fresh_submaps_count_,
min_covered_cells_count_); min_covered_cells_count_);

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

@ -807,12 +807,12 @@ std::vector<SubmapId> PoseGraph2D::TrimmingHandle::GetSubmapIds(
return submap_ids; return submap_ids;
} }
MapById<NodeId, TrajectoryNode> const MapById<NodeId, TrajectoryNode>&
PoseGraph2D::TrimmingHandle::GetTrajectoryNodes() const { PoseGraph2D::TrimmingHandle::GetTrajectoryNodes() const {
return parent_->trajectory_nodes_; return parent_->trajectory_nodes_;
} }
std::vector<PoseGraphInterface::Constraint> const std::vector<PoseGraphInterface::Constraint>&
PoseGraph2D::TrimmingHandle::GetConstraints() const { PoseGraph2D::TrimmingHandle::GetConstraints() const {
return parent_->constraints_; return parent_->constraints_;
} }

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

@ -284,10 +284,10 @@ class PoseGraph2D : public PoseGraph {
std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override; std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
const override REQUIRES(parent_->mutex_); const override REQUIRES(parent_->mutex_);
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
REQUIRES(parent_->mutex_);
std::vector<PoseGraphInterface::Constraint> GetConstraints() const override
REQUIRES(parent_->mutex_); REQUIRES(parent_->mutex_);
const std::vector<PoseGraphInterface::Constraint>& GetConstraints()
const override REQUIRES(parent_->mutex_);
void MarkSubmapAsTrimmed(const SubmapId& submap_id) void MarkSubmapAsTrimmed(const SubmapId& submap_id)
REQUIRES(parent_->mutex_) override; REQUIRES(parent_->mutex_) override;
bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_); bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_);

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

@ -836,12 +836,12 @@ PoseGraph3D::TrimmingHandle::GetAllSubmapData() const {
return parent_->GetSubmapDataUnderLock(); return parent_->GetSubmapDataUnderLock();
} }
MapById<NodeId, TrajectoryNode> const MapById<NodeId, TrajectoryNode>&
PoseGraph3D::TrimmingHandle::GetTrajectoryNodes() const { PoseGraph3D::TrimmingHandle::GetTrajectoryNodes() const {
return parent_->trajectory_nodes_; return parent_->trajectory_nodes_;
} }
std::vector<PoseGraphInterface::Constraint> const std::vector<PoseGraphInterface::Constraint>&
PoseGraph3D::TrimmingHandle::GetConstraints() const { PoseGraph3D::TrimmingHandle::GetConstraints() const {
return parent_->constraints_; return parent_->constraints_;
} }

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

@ -289,10 +289,10 @@ class PoseGraph3D : public PoseGraph {
std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override; std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
const override REQUIRES(parent_->mutex_); const override REQUIRES(parent_->mutex_);
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
REQUIRES(parent_->mutex_);
std::vector<PoseGraphInterface::Constraint> GetConstraints() const override
REQUIRES(parent_->mutex_); REQUIRES(parent_->mutex_);
const std::vector<PoseGraphInterface::Constraint>& GetConstraints()
const override REQUIRES(parent_->mutex_);
void MarkSubmapAsTrimmed(const SubmapId& submap_id) void MarkSubmapAsTrimmed(const SubmapId& submap_id)
REQUIRES(parent_->mutex_) override; REQUIRES(parent_->mutex_) override;
bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_); bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_);

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

@ -72,7 +72,7 @@ class FakeTrimmable : public Trimmable {
return &trajectory_nodes_; return &trajectory_nodes_;
} }
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override { const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override {
return trajectory_nodes_; return trajectory_nodes_;
} }
@ -85,7 +85,8 @@ class FakeTrimmable : public Trimmable {
return &constraints_; return &constraints_;
} }
std::vector<PoseGraphInterface::Constraint> GetConstraints() const override { const std::vector<PoseGraphInterface::Constraint>& GetConstraints()
const override {
return constraints_; return constraints_;
} }

4
cartographer/mapping/pose_graph_trimmer.h
View File

@ -34,8 +34,8 @@ class Trimmable {
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() virtual MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
const = 0; const = 0;
virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const = 0; virtual const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const = 0;
virtual std::vector<PoseGraphInterface::Constraint> GetConstraints() virtual const std::vector<PoseGraphInterface::Constraint>& GetConstraints()
const = 0; const = 0;
// Marks 'submap_id' and corresponding intra-submap nodes as trimmed. They // Marks 'submap_id' and corresponding intra-submap nodes as trimmed. They