From a82a62f8a0ae74be9cc109f79395b827edecb1d4 Mon Sep 17 00:00:00 2001 From: Kevin Daun Date: Mon, 16 Apr 2018 18:19:59 +0200 Subject: [PATCH] Replace Submap2D grid member by unique_ptr (#1073) - replace the `ProbabilityGrid` member of `Submap2D` by `unique_ptr` - allow `Submap2D` to handle `proto::Submap2D` without a defined grid field - resolve https://github.com/googlecartographer/cartographer/issues/1071 --- cartographer/mapping/2d/submap_2d.cc | 36 +++++++++++++++++----------- cartographer/mapping/2d/submap_2d.h | 4 ++-- 2 files changed, 24 insertions(+), 16 deletions(-) diff --git a/cartographer/mapping/2d/submap_2d.cc b/cartographer/mapping/2d/submap_2d.cc index a6d9723..b6b0cb6 100644 --- a/cartographer/mapping/2d/submap_2d.cc +++ b/cartographer/mapping/2d/submap_2d.cc @@ -65,11 +65,14 @@ proto::SubmapsOptions2D CreateSubmapsOptions2D( Submap2D::Submap2D(const MapLimits& limits, const Eigen::Vector2f& origin) : Submap(transform::Rigid3d::Translation( Eigen::Vector3d(origin.x(), origin.y(), 0.))), - probability_grid_(limits) {} + probability_grid_(common::make_unique(limits)) {} Submap2D::Submap2D(const proto::Submap2D& proto) - : Submap(transform::ToRigid3(proto.local_pose())), - probability_grid_(ProbabilityGrid(proto.grid())) { + : Submap(transform::ToRigid3(proto.local_pose())) { + if (proto.has_grid()) { + CHECK(proto.grid().has_probability_grid_2d()); + probability_grid_ = common::make_unique(proto.grid()); + } set_num_range_data(proto.num_range_data()); set_finished(proto.finished()); } @@ -81,7 +84,8 @@ void Submap2D::ToProto(proto::Submap* const proto, submap_2d->set_num_range_data(num_range_data()); submap_2d->set_finished(finished()); if (include_probability_grid_data) { - *submap_2d->mutable_grid() = probability_grid_.ToProto(); + CHECK(probability_grid_); + *submap_2d->mutable_grid() = probability_grid_->ToProto(); } } @@ -90,23 +94,25 @@ void Submap2D::UpdateFromProto(const proto::Submap& proto) { const auto& submap_2d = proto.submap_2d(); set_num_range_data(submap_2d.num_range_data()); set_finished(submap_2d.finished()); - if (submap_2d.has_grid()) { - probability_grid_ = ProbabilityGrid(submap_2d.grid()); + if (proto.submap_2d().has_grid()) { + CHECK(proto.submap_2d().grid().has_probability_grid_2d()); + probability_grid_ = common::make_unique(submap_2d.grid()); } } void Submap2D::ToResponseProto( const transform::Rigid3d&, proto::SubmapQuery::Response* const response) const { + CHECK(probability_grid_); response->set_submap_version(num_range_data()); Eigen::Array2i offset; CellLimits limits; - probability_grid_.ComputeCroppedLimits(&offset, &limits); + probability_grid_->ComputeCroppedLimits(&offset, &limits); std::string cells; for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) { - if (probability_grid_.IsKnown(xy_index + offset)) { + if (probability_grid_->IsKnown(xy_index + offset)) { // We would like to add 'delta' but this is not possible using a value and // alpha. We use premultiplied alpha, so when 'delta' is positive we can // add it by setting 'alpha' to zero. If it is negative, we set 'value' to @@ -115,7 +121,7 @@ void Submap2D::ToResponseProto( // detect visually for the user, though. const int delta = 128 - ProbabilityToLogOddsInteger( - probability_grid_.GetProbability(xy_index + offset)); + probability_grid_->GetProbability(xy_index + offset)); const uint8 alpha = delta > 0 ? 0 : -delta; const uint8 value = delta > 0 ? delta : 0; cells.push_back(value); @@ -132,12 +138,12 @@ void Submap2D::ToResponseProto( texture->set_width(limits.num_x_cells); texture->set_height(limits.num_y_cells); - const double resolution = probability_grid_.limits().resolution(); + const double resolution = probability_grid_->limits().resolution(); texture->set_resolution(resolution); const double max_x = - probability_grid_.limits().max().x() - resolution * offset.y(); + probability_grid_->limits().max().x() - resolution * offset.y(); const double max_y = - probability_grid_.limits().max().y() - resolution * offset.x(); + probability_grid_->limits().max().y() - resolution * offset.x(); *texture->mutable_slice_pose() = transform::ToProto( local_pose().inverse() * transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.))); @@ -145,14 +151,16 @@ void Submap2D::ToResponseProto( void Submap2D::InsertRangeData(const sensor::RangeData& range_data, const RangeDataInserter2D& range_data_inserter) { + CHECK(probability_grid_); CHECK(!finished()); - range_data_inserter.Insert(range_data, &probability_grid_); + range_data_inserter.Insert(range_data, probability_grid_.get()); set_num_range_data(num_range_data() + 1); } void Submap2D::Finish() { + CHECK(probability_grid_); CHECK(!finished()); - probability_grid_ = ComputeCroppedProbabilityGrid(probability_grid_); + *probability_grid_ = ComputeCroppedProbabilityGrid(*probability_grid_); set_finished(true); } diff --git a/cartographer/mapping/2d/submap_2d.h b/cartographer/mapping/2d/submap_2d.h index ba35e4c..7772e0a 100644 --- a/cartographer/mapping/2d/submap_2d.h +++ b/cartographer/mapping/2d/submap_2d.h @@ -54,7 +54,7 @@ class Submap2D : public Submap { void ToResponseProto(const transform::Rigid3d& global_submap_pose, proto::SubmapQuery::Response* response) const override; - const ProbabilityGrid& probability_grid() const { return probability_grid_; } + const ProbabilityGrid& probability_grid() const { return *probability_grid_; } // Insert 'range_data' into this submap using 'range_data_inserter'. The // submap must not be finished yet. @@ -63,7 +63,7 @@ class Submap2D : public Submap { void Finish(); private: - ProbabilityGrid probability_grid_; + std::unique_ptr probability_grid_; }; // Except during initialization when only a single submap exists, there are