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();
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));
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(
ComputeSubmapFreshness(submap_data, trajectory_nodes, constraints),
submap_data, &coverage_grid);
submap_freshness, submap_data, &coverage_grid);
const std::vector<SubmapId> submap_ids_to_remove =
FindSubmapIdsToTrim(coverage_grid, all_submap_ids, fresh_submaps_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;
}
MapById<NodeId, TrajectoryNode>
const MapById<NodeId, TrajectoryNode>&
PoseGraph2D::TrimmingHandle::GetTrajectoryNodes() const {
return parent_->trajectory_nodes_;
}
std::vector<PoseGraphInterface::Constraint>
const std::vector<PoseGraphInterface::Constraint>&
PoseGraph2D::TrimmingHandle::GetConstraints() const {
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;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
const override REQUIRES(parent_->mutex_);
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override
REQUIRES(parent_->mutex_);
std::vector<PoseGraphInterface::Constraint> GetConstraints() const override
const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
REQUIRES(parent_->mutex_);
const std::vector<PoseGraphInterface::Constraint>& GetConstraints()
const override REQUIRES(parent_->mutex_);
void MarkSubmapAsTrimmed(const SubmapId& submap_id)
REQUIRES(parent_->mutex_) override;
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();
}
MapById<NodeId, TrajectoryNode>
const MapById<NodeId, TrajectoryNode>&
PoseGraph3D::TrimmingHandle::GetTrajectoryNodes() const {
return parent_->trajectory_nodes_;
}
std::vector<PoseGraphInterface::Constraint>
const std::vector<PoseGraphInterface::Constraint>&
PoseGraph3D::TrimmingHandle::GetConstraints() const {
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;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
const override REQUIRES(parent_->mutex_);
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override
REQUIRES(parent_->mutex_);
std::vector<PoseGraphInterface::Constraint> GetConstraints() const override
const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
REQUIRES(parent_->mutex_);
const std::vector<PoseGraphInterface::Constraint>& GetConstraints()
const override REQUIRES(parent_->mutex_);
void MarkSubmapAsTrimmed(const SubmapId& submap_id)
REQUIRES(parent_->mutex_) override;
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_;
}
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override {
const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override {
return trajectory_nodes_;
}
@ -85,7 +85,8 @@ class FakeTrimmable : public Trimmable {
return &constraints_;
}
std::vector<PoseGraphInterface::Constraint> GetConstraints() const override {
const std::vector<PoseGraphInterface::Constraint>& GetConstraints()
const override {
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 MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
const = 0;
virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const = 0;
virtual std::vector<PoseGraphInterface::Constraint> GetConstraints()
virtual const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const = 0;
virtual const std::vector<PoseGraphInterface::Constraint>& GetConstraints()
const = 0;
// Marks 'submap_id' and corresponding intra-submap nodes as trimmed. They