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

Remove friends declaration in Submap. (#350)

master
Holger Rapp 2017-06-21 12:49:44 +02:00 committed by GitHub
parent 0fbdf6d0ec
commit 16636cd4e1
5 changed files with 57 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/submaps.h
View File

@ -65,7 +65,7 @@ class Submap {
transform::Rigid3d local_pose() const { return local_pose_; }
// Number of RangeData inserted.
size_t num_range_data() const { return num_range_data_; }
int num_range_data() const { return num_range_data_; }
// Fills data into the 'response'.
virtual void ToResponseProto(

30
cartographer/mapping_2d/submaps.cc
View File

@ -151,6 +151,19 @@ void Submap::ToResponseProto(
transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
}
void Submap::InsertRangeData(const sensor::RangeData& range_data,
const RangeDataInserter& range_data_inserter) {
CHECK(!finished_);
range_data_inserter.Insert(range_data, &probability_grid_);
++num_range_data_;
}
void Submap::Finish() {
CHECK(!finished_);
probability_grid_ = ComputeCroppedProbabilityGrid(probability_grid_);
finished_ = true;
}
ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options)
: options_(options),
range_data_inserter_(options.range_data_inserter_options()) {
@ -161,11 +174,9 @@ ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options)
void ActiveSubmaps::InsertRangeData(const sensor::RangeData& range_data) {
for (auto& submap : submaps_) {
CHECK(!submap->finished_);
range_data_inserter_.Insert(range_data, &submap->probability_grid_);
++submap->num_range_data_;
submap->InsertRangeData(range_data, range_data_inserter_);
}
if (submaps_.back()->num_range_data_ == options_.num_range_data()) {
if (submaps_.back()->num_range_data() == options_.num_range_data()) {
AddSubmap(range_data.origin.head<2>());
}
}
@ -177,17 +188,12 @@ std::vector<std::shared_ptr<Submap>> ActiveSubmaps::submaps() const {
int ActiveSubmaps::matching_index() const { return matching_submap_index_; }
void ActiveSubmaps::FinishSubmap() {
// Crop the finished Submap before inserting a new Submap to reduce peak
// memory usage a bit.
Submap* submap = submaps_.front().get();
CHECK(!submap->finished_);
submap->probability_grid_ =
ComputeCroppedProbabilityGrid(submap->probability_grid_);
submap->finished_ = true;
submap->Finish();
if (options_.output_debug_images()) {
// Output the Submap that won't be changed from now on.
WriteDebugImage("submap" + std::to_string(matching_submap_index_) + ".webp",
submaps_.front()->probability_grid_);
submaps_.front()->probability_grid());
}
++matching_submap_index_;
submaps_.erase(submaps_.begin());
@ -195,6 +201,8 @@ void ActiveSubmaps::FinishSubmap() {
void ActiveSubmaps::AddSubmap(const Eigen::Vector2f& origin) {
if (submaps_.size() > 1) {
// This will crop the finished Submap before inserting a new Submap to
// reduce peak memory usage a bit.
FinishSubmap();
}
const int num_cells_per_dimension =

9
cartographer/mapping_2d/submaps.h
View File

@ -52,10 +52,13 @@ class Submap : public mapping::Submap {
const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* response) const override;
private:
// TODO(hrapp): Remove friend declaration.
friend class ActiveSubmaps;
// Insert 'range_data' into this submap using 'range_data_inserter'. The
// submap must not be finished yet.
void InsertRangeData(const sensor::RangeData& range_data,
const RangeDataInserter& range_data_inserter);
void Finish();
private:
ProbabilityGrid probability_grid_;
bool finished_ = false;
};

37
cartographer/mapping_3d/submaps.cc
View File

@ -360,6 +360,26 @@ void Submap::ToResponseProto(
global_submap_pose.translation().z())));
}
void Submap::InsertRangeData(const sensor::RangeData& range_data,
const RangeDataInserter& range_data_inserter,
const int high_resolution_max_range) {
CHECK(!finished_);
const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
range_data, local_pose().inverse().cast<float>());
range_data_inserter.Insert(
FilterRangeDataByMaxRange(transformed_range_data,
high_resolution_max_range),
&high_resolution_hybrid_grid_);
range_data_inserter.Insert(transformed_range_data,
&low_resolution_hybrid_grid_);
++num_range_data_;
}
void Submap::Finish() {
CHECK(!finished_);
finished_ = true;
}
ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options)
: options_(options),
range_data_inserter_(options.range_data_inserter_options()) {
@ -381,17 +401,10 @@ void ActiveSubmaps::InsertRangeData(
const sensor::RangeData& range_data,
const Eigen::Quaterniond& gravity_alignment) {
for (auto& submap : submaps_) {
const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
range_data, submap->local_pose().inverse().cast<float>());
range_data_inserter_.Insert(
FilterRangeDataByMaxRange(transformed_range_data,
options_.high_resolution_max_range()),
&submap->high_resolution_hybrid_grid_);
range_data_inserter_.Insert(transformed_range_data,
&submap->low_resolution_hybrid_grid_);
++submap->num_range_data_;
submap->InsertRangeData(range_data, range_data_inserter_,
options_.high_resolution_max_range());
}
if (submaps_.back()->num_range_data_ == options_.num_range_data()) {
if (submaps_.back()->num_range_data() == options_.num_range_data()) {
AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
gravity_alignment));
}
@ -399,9 +412,7 @@ void ActiveSubmaps::InsertRangeData(
void ActiveSubmaps::AddSubmap(const transform::Rigid3d& local_pose) {
if (submaps_.size() > 1) {
Submap* submap = submaps_.front().get();
CHECK(!submap->finished_);
submap->finished_ = true;
submaps_.front()->Finish();
++matching_submap_index_;
submaps_.erase(submaps_.begin());
}

10
cartographer/mapping_3d/submaps.h
View File

@ -64,10 +64,14 @@ class Submap : public mapping::Submap {
const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* response) const override;
private:
// TODO(hrapp): Remove friend declaration.
friend class ActiveSubmaps;
// Insert 'range_data' into this submap using 'range_data_inserter'. The
// submap must not be finished yet.
void InsertRangeData(const sensor::RangeData& range_data,
const RangeDataInserter& range_data_inserter,
int high_resolution_max_range);
void Finish();
private:
HybridGrid high_resolution_hybrid_grid_;
HybridGrid low_resolution_hybrid_grid_;
bool finished_ = false;