Remove friends declaration in Submap. (#350)
parent
0fbdf6d0ec
commit
16636cd4e1
|
@ -65,7 +65,7 @@ class Submap {
|
||||||
transform::Rigid3d local_pose() const { return local_pose_; }
|
transform::Rigid3d local_pose() const { return local_pose_; }
|
||||||
|
|
||||||
// Number of RangeData inserted.
|
// 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'.
|
// Fills data into the 'response'.
|
||||||
virtual void ToResponseProto(
|
virtual void ToResponseProto(
|
||||||
|
|
|
@ -151,6 +151,19 @@ void Submap::ToResponseProto(
|
||||||
transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
|
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)
|
ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options)
|
||||||
: options_(options),
|
: options_(options),
|
||||||
range_data_inserter_(options.range_data_inserter_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) {
|
void ActiveSubmaps::InsertRangeData(const sensor::RangeData& range_data) {
|
||||||
for (auto& submap : submaps_) {
|
for (auto& submap : submaps_) {
|
||||||
CHECK(!submap->finished_);
|
submap->InsertRangeData(range_data, range_data_inserter_);
|
||||||
range_data_inserter_.Insert(range_data, &submap->probability_grid_);
|
|
||||||
++submap->num_range_data_;
|
|
||||||
}
|
}
|
||||||
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>());
|
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_; }
|
int ActiveSubmaps::matching_index() const { return matching_submap_index_; }
|
||||||
|
|
||||||
void ActiveSubmaps::FinishSubmap() {
|
void ActiveSubmaps::FinishSubmap() {
|
||||||
// Crop the finished Submap before inserting a new Submap to reduce peak
|
|
||||||
// memory usage a bit.
|
|
||||||
Submap* submap = submaps_.front().get();
|
Submap* submap = submaps_.front().get();
|
||||||
CHECK(!submap->finished_);
|
submap->Finish();
|
||||||
submap->probability_grid_ =
|
|
||||||
ComputeCroppedProbabilityGrid(submap->probability_grid_);
|
|
||||||
submap->finished_ = true;
|
|
||||||
if (options_.output_debug_images()) {
|
if (options_.output_debug_images()) {
|
||||||
// Output the Submap that won't be changed from now on.
|
// Output the Submap that won't be changed from now on.
|
||||||
WriteDebugImage("submap" + std::to_string(matching_submap_index_) + ".webp",
|
WriteDebugImage("submap" + std::to_string(matching_submap_index_) + ".webp",
|
||||||
submaps_.front()->probability_grid_);
|
submaps_.front()->probability_grid());
|
||||||
}
|
}
|
||||||
++matching_submap_index_;
|
++matching_submap_index_;
|
||||||
submaps_.erase(submaps_.begin());
|
submaps_.erase(submaps_.begin());
|
||||||
|
@ -195,6 +201,8 @@ void ActiveSubmaps::FinishSubmap() {
|
||||||
|
|
||||||
void ActiveSubmaps::AddSubmap(const Eigen::Vector2f& origin) {
|
void ActiveSubmaps::AddSubmap(const Eigen::Vector2f& origin) {
|
||||||
if (submaps_.size() > 1) {
|
if (submaps_.size() > 1) {
|
||||||
|
// This will crop the finished Submap before inserting a new Submap to
|
||||||
|
// reduce peak memory usage a bit.
|
||||||
FinishSubmap();
|
FinishSubmap();
|
||||||
}
|
}
|
||||||
const int num_cells_per_dimension =
|
const int num_cells_per_dimension =
|
||||||
|
|
|
@ -52,10 +52,13 @@ class Submap : public mapping::Submap {
|
||||||
const transform::Rigid3d& global_submap_pose,
|
const transform::Rigid3d& global_submap_pose,
|
||||||
mapping::proto::SubmapQuery::Response* response) const override;
|
mapping::proto::SubmapQuery::Response* response) const override;
|
||||||
|
|
||||||
private:
|
// Insert 'range_data' into this submap using 'range_data_inserter'. The
|
||||||
// TODO(hrapp): Remove friend declaration.
|
// submap must not be finished yet.
|
||||||
friend class ActiveSubmaps;
|
void InsertRangeData(const sensor::RangeData& range_data,
|
||||||
|
const RangeDataInserter& range_data_inserter);
|
||||||
|
void Finish();
|
||||||
|
|
||||||
|
private:
|
||||||
ProbabilityGrid probability_grid_;
|
ProbabilityGrid probability_grid_;
|
||||||
bool finished_ = false;
|
bool finished_ = false;
|
||||||
};
|
};
|
||||||
|
|
|
@ -360,6 +360,26 @@ void Submap::ToResponseProto(
|
||||||
global_submap_pose.translation().z())));
|
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)
|
ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options)
|
||||||
: options_(options),
|
: options_(options),
|
||||||
range_data_inserter_(options.range_data_inserter_options()) {
|
range_data_inserter_(options.range_data_inserter_options()) {
|
||||||
|
@ -381,17 +401,10 @@ void ActiveSubmaps::InsertRangeData(
|
||||||
const sensor::RangeData& range_data,
|
const sensor::RangeData& range_data,
|
||||||
const Eigen::Quaterniond& gravity_alignment) {
|
const Eigen::Quaterniond& gravity_alignment) {
|
||||||
for (auto& submap : submaps_) {
|
for (auto& submap : submaps_) {
|
||||||
const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
|
submap->InsertRangeData(range_data, range_data_inserter_,
|
||||||
range_data, submap->local_pose().inverse().cast<float>());
|
options_.high_resolution_max_range());
|
||||||
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_;
|
|
||||||
}
|
}
|
||||||
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>(),
|
AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
|
||||||
gravity_alignment));
|
gravity_alignment));
|
||||||
}
|
}
|
||||||
|
@ -399,9 +412,7 @@ void ActiveSubmaps::InsertRangeData(
|
||||||
|
|
||||||
void ActiveSubmaps::AddSubmap(const transform::Rigid3d& local_pose) {
|
void ActiveSubmaps::AddSubmap(const transform::Rigid3d& local_pose) {
|
||||||
if (submaps_.size() > 1) {
|
if (submaps_.size() > 1) {
|
||||||
Submap* submap = submaps_.front().get();
|
submaps_.front()->Finish();
|
||||||
CHECK(!submap->finished_);
|
|
||||||
submap->finished_ = true;
|
|
||||||
++matching_submap_index_;
|
++matching_submap_index_;
|
||||||
submaps_.erase(submaps_.begin());
|
submaps_.erase(submaps_.begin());
|
||||||
}
|
}
|
||||||
|
|
|
@ -64,10 +64,14 @@ class Submap : public mapping::Submap {
|
||||||
const transform::Rigid3d& global_submap_pose,
|
const transform::Rigid3d& global_submap_pose,
|
||||||
mapping::proto::SubmapQuery::Response* response) const override;
|
mapping::proto::SubmapQuery::Response* response) const override;
|
||||||
|
|
||||||
private:
|
// Insert 'range_data' into this submap using 'range_data_inserter'. The
|
||||||
// TODO(hrapp): Remove friend declaration.
|
// submap must not be finished yet.
|
||||||
friend class ActiveSubmaps;
|
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 high_resolution_hybrid_grid_;
|
||||||
HybridGrid low_resolution_hybrid_grid_;
|
HybridGrid low_resolution_hybrid_grid_;
|
||||||
bool finished_ = false;
|
bool finished_ = false;
|
||||||
|
|
Loading…
Reference in New Issue