Replace Submap2D grid member by unique_ptr (#1073)

- replace the `ProbabilityGrid` member of `Submap2D` by  `unique_ptr<ProbabilityGrid>`
- allow `Submap2D` to handle `proto::Submap2D` without a defined grid field
- resolve https://github.com/googlecartographer/cartographer/issues/1071
master
Kevin Daun 2018-04-16 18:19:59 +02:00 committed by Wally B. Feed
parent c20f22e986
commit a82a62f8a0
2 changed files with 24 additions and 16 deletions

View File

@ -65,11 +65,14 @@ proto::SubmapsOptions2D CreateSubmapsOptions2D(
Submap2D::Submap2D(const MapLimits& limits, const Eigen::Vector2f& origin) Submap2D::Submap2D(const MapLimits& limits, const Eigen::Vector2f& origin)
: Submap(transform::Rigid3d::Translation( : Submap(transform::Rigid3d::Translation(
Eigen::Vector3d(origin.x(), origin.y(), 0.))), Eigen::Vector3d(origin.x(), origin.y(), 0.))),
probability_grid_(limits) {} probability_grid_(common::make_unique<ProbabilityGrid>(limits)) {}
Submap2D::Submap2D(const proto::Submap2D& proto) Submap2D::Submap2D(const proto::Submap2D& proto)
: Submap(transform::ToRigid3(proto.local_pose())), : Submap(transform::ToRigid3(proto.local_pose())) {
probability_grid_(ProbabilityGrid(proto.grid())) { if (proto.has_grid()) {
CHECK(proto.grid().has_probability_grid_2d());
probability_grid_ = common::make_unique<ProbabilityGrid>(proto.grid());
}
set_num_range_data(proto.num_range_data()); set_num_range_data(proto.num_range_data());
set_finished(proto.finished()); 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_num_range_data(num_range_data());
submap_2d->set_finished(finished()); submap_2d->set_finished(finished());
if (include_probability_grid_data) { 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(); const auto& submap_2d = proto.submap_2d();
set_num_range_data(submap_2d.num_range_data()); set_num_range_data(submap_2d.num_range_data());
set_finished(submap_2d.finished()); set_finished(submap_2d.finished());
if (submap_2d.has_grid()) { if (proto.submap_2d().has_grid()) {
probability_grid_ = ProbabilityGrid(submap_2d.grid()); CHECK(proto.submap_2d().grid().has_probability_grid_2d());
probability_grid_ = common::make_unique<ProbabilityGrid>(submap_2d.grid());
} }
} }
void Submap2D::ToResponseProto( void Submap2D::ToResponseProto(
const transform::Rigid3d&, const transform::Rigid3d&,
proto::SubmapQuery::Response* const response) const { proto::SubmapQuery::Response* const response) const {
CHECK(probability_grid_);
response->set_submap_version(num_range_data()); response->set_submap_version(num_range_data());
Eigen::Array2i offset; Eigen::Array2i offset;
CellLimits limits; CellLimits limits;
probability_grid_.ComputeCroppedLimits(&offset, &limits); probability_grid_->ComputeCroppedLimits(&offset, &limits);
std::string cells; std::string cells;
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) { 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 // 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 // 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 // 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. // detect visually for the user, though.
const int delta = const int delta =
128 - ProbabilityToLogOddsInteger( 128 - ProbabilityToLogOddsInteger(
probability_grid_.GetProbability(xy_index + offset)); probability_grid_->GetProbability(xy_index + offset));
const uint8 alpha = delta > 0 ? 0 : -delta; const uint8 alpha = delta > 0 ? 0 : -delta;
const uint8 value = delta > 0 ? delta : 0; const uint8 value = delta > 0 ? delta : 0;
cells.push_back(value); cells.push_back(value);
@ -132,12 +138,12 @@ void Submap2D::ToResponseProto(
texture->set_width(limits.num_x_cells); texture->set_width(limits.num_x_cells);
texture->set_height(limits.num_y_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); texture->set_resolution(resolution);
const double max_x = const double max_x =
probability_grid_.limits().max().x() - resolution * offset.y(); probability_grid_->limits().max().x() - resolution * offset.y();
const double max_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( *texture->mutable_slice_pose() = transform::ToProto(
local_pose().inverse() * local_pose().inverse() *
transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.))); 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, void Submap2D::InsertRangeData(const sensor::RangeData& range_data,
const RangeDataInserter2D& range_data_inserter) { const RangeDataInserter2D& range_data_inserter) {
CHECK(probability_grid_);
CHECK(!finished()); 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); set_num_range_data(num_range_data() + 1);
} }
void Submap2D::Finish() { void Submap2D::Finish() {
CHECK(probability_grid_);
CHECK(!finished()); CHECK(!finished());
probability_grid_ = ComputeCroppedProbabilityGrid(probability_grid_); *probability_grid_ = ComputeCroppedProbabilityGrid(*probability_grid_);
set_finished(true); set_finished(true);
} }

View File

@ -54,7 +54,7 @@ class Submap2D : public Submap {
void ToResponseProto(const transform::Rigid3d& global_submap_pose, void ToResponseProto(const transform::Rigid3d& global_submap_pose,
proto::SubmapQuery::Response* response) const override; 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 // Insert 'range_data' into this submap using 'range_data_inserter'. The
// submap must not be finished yet. // submap must not be finished yet.
@ -63,7 +63,7 @@ class Submap2D : public Submap {
void Finish(); void Finish();
private: private:
ProbabilityGrid probability_grid_; std::unique_ptr<ProbabilityGrid> probability_grid_;
}; };
// Except during initialization when only a single submap exists, there are // Except during initialization when only a single submap exists, there are