Allow trimmers fetch submap data. (#1024)

master
Alexander Belyaev 2018-03-28 12:07:28 +02:00 committed by GitHub
parent 962393074a
commit 8c6c584524
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 47 additions and 12 deletions

View File

@ -716,12 +716,7 @@ PoseGraph::SubmapData PoseGraph2D::GetSubmapData(const SubmapId& submap_id) {
MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph2D::GetAllSubmapData() {
common::MutexLocker locker(&mutex_);
MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
for (const auto& submap_id_data : submap_data_) {
submaps.Insert(submap_id_data.id,
GetSubmapDataUnderLock(submap_id_data.id));
}
return submaps;
return GetSubmapDataUnderLock();
}
MapById<SubmapId, PoseGraphInterface::SubmapPose>
@ -788,6 +783,11 @@ int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const {
return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
}
MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph2D::TrimmingHandle::GetAllSubmapData() const {
return parent_->GetSubmapDataUnderLock();
}
std::vector<SubmapId> PoseGraph2D::TrimmingHandle::GetSubmapIds(
int trajectory_id) const {
std::vector<SubmapId> submap_ids;
@ -860,5 +860,15 @@ void PoseGraph2D::TrimmingHandle::MarkSubmapAsTrimmed(
}
}
MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph2D::GetSubmapDataUnderLock() {
MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
for (const auto& submap_id_data : submap_data_) {
submaps.Insert(submap_id_data.id,
GetSubmapDataUnderLock(submap_id_data.id));
}
return submaps;
}
} // namespace mapping
} // namespace cartographer

View File

@ -153,6 +153,8 @@ class PoseGraph2D : public PoseGraph {
SubmapState state = SubmapState::kActive;
};
MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock();
// Handles a new work item.
void AddWorkItem(const std::function<void()>& work_item) REQUIRES(mutex_);
@ -279,6 +281,8 @@ class PoseGraph2D : public PoseGraph {
int num_submaps(int trajectory_id) const override;
std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
const override;
void MarkSubmapAsTrimmed(const SubmapId& submap_id)
REQUIRES(parent_->mutex_) override;
bool IsFinished(int trajectory_id) const override;

View File

@ -749,12 +749,7 @@ PoseGraph::SubmapData PoseGraph3D::GetSubmapData(const SubmapId& submap_id) {
MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph3D::GetAllSubmapData() {
common::MutexLocker locker(&mutex_);
MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
for (const auto& submap_id_data : submap_data_) {
submaps.Insert(submap_id_data.id,
GetSubmapDataUnderLock(submap_id_data.id));
}
return submaps;
return GetSubmapDataUnderLock();
}
MapById<SubmapId, PoseGraphInterface::SubmapPose>
@ -828,6 +823,10 @@ std::vector<SubmapId> PoseGraph3D::TrimmingHandle::GetSubmapIds(
}
return submap_ids;
}
MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph3D::TrimmingHandle::GetAllSubmapData() const {
return parent_->GetSubmapDataUnderLock();
}
bool PoseGraph3D::TrimmingHandle::IsFinished(const int trajectory_id) const {
return parent_->IsTrajectoryFinished(trajectory_id);
@ -891,5 +890,15 @@ void PoseGraph3D::TrimmingHandle::MarkSubmapAsTrimmed(
}
}
MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph3D::GetSubmapDataUnderLock() {
MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
for (const auto& submap_id_data : submap_data_) {
submaps.Insert(submap_id_data.id,
GetSubmapDataUnderLock(submap_id_data.id));
}
return submaps;
}
} // namespace mapping
} // namespace cartographer

View File

@ -158,6 +158,8 @@ class PoseGraph3D : public PoseGraph {
SubmapState state = SubmapState::kActive;
};
MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock();
// Handles a new work item.
void AddWorkItem(const std::function<void()>& work_item) REQUIRES(mutex_);
@ -284,6 +286,8 @@ class PoseGraph3D : public PoseGraph {
int num_submaps(int trajectory_id) const override;
std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
const override;
void MarkSubmapAsTrimmed(const SubmapId& submap_id)
REQUIRES(parent_->mutex_) override;
bool IsFinished(int trajectory_id) const override;

View File

@ -18,6 +18,7 @@
#define CARTOGRAPHER_MAPPING_POSE_GRAPH_TRIMMER_H_
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/pose_graph_interface.h"
namespace cartographer {
namespace mapping {
@ -31,6 +32,8 @@ class Trimmable {
virtual int num_submaps(int trajectory_id) const = 0;
virtual std::vector<SubmapId> GetSubmapIds(int trajectory_id) const = 0;
virtual MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
const = 0;
// Marks 'submap_id' and corresponding intra-submap nodes as trimmed. They
// will no longer take part in scan matching, loop closure, visualization.

View File

@ -42,6 +42,11 @@ class FakePoseGraph : public Trimmable {
return submaps_;
}
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
const override {
return {};
}
void MarkSubmapAsTrimmed(const SubmapId& submap_id) override {
trimmed_submaps_.push_back(submap_id);
}