Replace ProbabilityGrid in Submap2D by Grid2D (#1097)

cartographer/mapping/2d/grid_2d.h

@@ -21,6 +21,7 @@
 
 #include "cartographer/mapping/2d/map_limits.h"
 #include "cartographer/mapping/2d/proto/grid_2d.pb.h"
+#include "cartographer/mapping/proto/submap_visualization.pb.h"
 
 namespace cartographer {
 namespace mapping {
@@ -58,8 +59,14 @@ class Grid2D {
   // after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
   void GrowLimits(const Eigen::Vector2f& point);
 
+  virtual std::unique_ptr<Grid2D> ComputeCroppedGrid() const = 0;
+
   virtual proto::Grid2D ToProto() const;
 
+  virtual bool DrawToSubmapTexture(
+      proto::SubmapQuery::Response::SubmapTexture* const texture,
+      transform::Rigid3d local_pose) const = 0;
+
  protected:
   const std::vector<uint16>& correspondence_cost_cells() const {
     return correspondence_cost_cells_;

cartographer/mapping/2d/probability_grid.cc

@@ -17,7 +17,9 @@
 
 #include <limits>
 
+#include "cartographer/common/make_unique.h"
 #include "cartographer/mapping/probability_values.h"
+#include "cartographer/mapping/submaps.h"
 
 namespace cartographer {
 namespace mapping {
@@ -76,5 +78,65 @@ proto::Grid2D ProbabilityGrid::ToProto() const {
   return result;
 }
 
+std::unique_ptr<Grid2D> ProbabilityGrid::ComputeCroppedGrid() const {
+  Eigen::Array2i offset;
+  CellLimits cell_limits;
+  ComputeCroppedLimits(&offset, &cell_limits);
+  const double resolution = limits().resolution();
+  const Eigen::Vector2d max =
+      limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x());
+  std::unique_ptr<ProbabilityGrid> cropped_grid =
+      common::make_unique<ProbabilityGrid>(
+          MapLimits(resolution, max, cell_limits));
+  for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
+    if (!IsKnown(xy_index + offset)) continue;
+    cropped_grid->SetProbability(xy_index, GetProbability(xy_index + offset));
+  }
+
+  return std::unique_ptr<Grid2D>(cropped_grid.release());
+}
+
+bool ProbabilityGrid::DrawToSubmapTexture(
+    proto::SubmapQuery::Response::SubmapTexture* const texture,
+    transform::Rigid3d local_pose) const {
+  Eigen::Array2i offset;
+  CellLimits cell_limits;
+  ComputeCroppedLimits(&offset, &cell_limits);
+
+  std::string cells;
+  for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
+    if (!IsKnown(xy_index + offset)) {
+      cells.push_back(0 /* unknown log odds value */);
+      cells.push_back(0 /* alpha */);
+      continue;
+    }
+    // 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
+    // zero, and use 'alpha' to subtract. This is only correct when the pixel
+    // is currently white, so walls will look too gray. This should be hard to
+    // detect visually for the user, though.
+    const int delta =
+        128 - ProbabilityToLogOddsInteger(GetProbability(xy_index + offset));
+    const uint8 alpha = delta > 0 ? 0 : -delta;
+    const uint8 value = delta > 0 ? delta : 0;
+    cells.push_back(value);
+    cells.push_back((value || alpha) ? alpha : 1);
+  }
+
+  common::FastGzipString(cells, texture->mutable_cells());
+  texture->set_width(cell_limits.num_x_cells);
+  texture->set_height(cell_limits.num_y_cells);
+  const double resolution = limits().resolution();
+  texture->set_resolution(resolution);
+  const double max_x = limits().max().x() - resolution * offset.y();
+  const double max_y = 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.)));
+
+  return true;
+}
+
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/2d/probability_grid.h

@@ -52,6 +52,10 @@ class ProbabilityGrid : public Grid2D {
   float GetProbability(const Eigen::Array2i& cell_index) const;
 
   virtual proto::Grid2D ToProto() const override;
+  virtual std::unique_ptr<Grid2D> ComputeCroppedGrid() const override;
+  virtual bool DrawToSubmapTexture(
+      proto::SubmapQuery::Response::SubmapTexture* const texture,
+      transform::Rigid3d local_pose) const override;
 };
 
 }  // namespace mapping

cartographer/mapping/2d/range_data_inserter_2d.cc

@@ -52,14 +52,13 @@ RangeDataInserter2D::RangeDataInserter2D(
       miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
           Odds(options.miss_probability()))) {}
 
-void RangeDataInserter2D::Insert(
+void RangeDataInserter2D::Insert(const sensor::RangeData& range_data,
-    const sensor::RangeData& range_data,
+                                 Grid2D* const grid) const {
-    ProbabilityGrid* const probability_grid) const {
   // By not finishing the update after hits are inserted, we give hits priority
   // (i.e. no hits will be ignored because of a miss in the same cell).
   CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),
-           CHECK_NOTNULL(probability_grid));
+           CHECK_NOTNULL(static_cast<ProbabilityGrid*>(grid)));
-  probability_grid->FinishUpdate();
+  grid->FinishUpdate();
 }
 
 }  // namespace mapping

cartographer/mapping/2d/range_data_inserter_2d.h

@@ -43,8 +43,7 @@ class RangeDataInserter2D {
   RangeDataInserter2D& operator=(const RangeDataInserter2D&) = delete;
 
   // Inserts 'range_data' into 'probability_grid'.
-  void Insert(const sensor::RangeData& range_data,
+  void Insert(const sensor::RangeData& range_data, Grid2D* grid) const;
-              ProbabilityGrid* probability_grid) const;
 
  private:
   const proto::RangeDataInserterOptions2D options_;

cartographer/mapping/2d/submap_2d.cc

@@ -30,25 +30,6 @@
 namespace cartographer {
 namespace mapping {
 
-ProbabilityGrid ComputeCroppedProbabilityGrid(
-    const ProbabilityGrid& probability_grid) {
-  Eigen::Array2i offset;
-  CellLimits limits;
-  probability_grid.ComputeCroppedLimits(&offset, &limits);
-  const double resolution = probability_grid.limits().resolution();
-  const Eigen::Vector2d max =
-      probability_grid.limits().max() -
-      resolution * Eigen::Vector2d(offset.y(), offset.x());
-  ProbabilityGrid cropped_grid(MapLimits(resolution, max, limits));
-  for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
-    if (probability_grid.IsKnown(xy_index + offset)) {
-      cropped_grid.SetProbability(
-          xy_index, probability_grid.GetProbability(xy_index + offset));
-    }
-  }
-  return cropped_grid;
-}
-
 proto::SubmapsOptions2D CreateSubmapsOptions2D(
     common::LuaParameterDictionary* const parameter_dictionary) {
   proto::SubmapsOptions2D options;
@@ -65,13 +46,13 @@ 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_(common::make_unique<ProbabilityGrid>(limits)) {}
+      grid_(common::make_unique<ProbabilityGrid>(limits)) {}
 
 Submap2D::Submap2D(const proto::Submap2D& proto)
     : Submap(transform::ToRigid3(proto.local_pose())) {
   if (proto.has_grid()) {
     CHECK(proto.grid().has_probability_grid_2d());
-    probability_grid_ = common::make_unique<ProbabilityGrid>(proto.grid());
+    grid_ = common::make_unique<ProbabilityGrid>(proto.grid());
   }
   set_num_range_data(proto.num_range_data());
   set_finished(proto.finished());
@@ -84,8 +65,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) {
-    CHECK(probability_grid_);
+    CHECK(grid_);
-    *submap_2d->mutable_grid() = probability_grid_->ToProto();
+    *submap_2d->mutable_grid() = grid_->ToProto();
   }
 }
 
@@ -96,71 +77,32 @@ void Submap2D::UpdateFromProto(const proto::Submap& proto) {
   set_finished(submap_2d.finished());
   if (proto.submap_2d().has_grid()) {
     CHECK(proto.submap_2d().grid().has_probability_grid_2d());
-    probability_grid_ = common::make_unique<ProbabilityGrid>(submap_2d.grid());
+    grid_ = common::make_unique<ProbabilityGrid>(submap_2d.grid());
   }
 }
 
 void Submap2D::ToResponseProto(
     const transform::Rigid3d&,
     proto::SubmapQuery::Response* const response) const {
-  if (!probability_grid_) return;
+  if (!grid_) return;
   response->set_submap_version(num_range_data());
-
-  Eigen::Array2i offset;
-  CellLimits limits;
-  probability_grid_->ComputeCroppedLimits(&offset, &limits);
-
-  std::string cells;
-  for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
-    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
-      // zero, and use 'alpha' to subtract. This is only correct when the pixel
-      // is currently white, so walls will look too gray. This should be hard to
-      // detect visually for the user, though.
-      const int delta =
-          128 - ProbabilityToLogOddsInteger(
-                    probability_grid_->GetProbability(xy_index + offset));
-      const uint8 alpha = delta > 0 ? 0 : -delta;
-      const uint8 value = delta > 0 ? delta : 0;
-      cells.push_back(value);
-      cells.push_back((value || alpha) ? alpha : 1);
-    } else {
-      constexpr uint8 kUnknownLogOdds = 0;
-      cells.push_back(static_cast<uint8>(kUnknownLogOdds));  // value
-      cells.push_back(0);                                    // alpha
-    }
-  }
   proto::SubmapQuery::Response::SubmapTexture* const texture =
       response->add_textures();
-  common::FastGzipString(cells, texture->mutable_cells());
+  grid()->DrawToSubmapTexture(texture, local_pose());
-
-  texture->set_width(limits.num_x_cells);
-  texture->set_height(limits.num_y_cells);
-  const double resolution = probability_grid_->limits().resolution();
-  texture->set_resolution(resolution);
-  const double max_x =
-      probability_grid_->limits().max().x() - resolution * offset.y();
-  const double max_y =
-      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.)));
 }
 
 void Submap2D::InsertRangeData(const sensor::RangeData& range_data,
                                const RangeDataInserter2D& range_data_inserter) {
-  CHECK(probability_grid_);
+  CHECK(grid_);
   CHECK(!finished());
-  range_data_inserter.Insert(range_data, probability_grid_.get());
+  range_data_inserter.Insert(range_data, grid_.get());
   set_num_range_data(num_range_data() + 1);
 }
 
 void Submap2D::Finish() {
-  CHECK(probability_grid_);
+  CHECK(grid_);
   CHECK(!finished());
-  *probability_grid_ = ComputeCroppedProbabilityGrid(*probability_grid_);
+  grid_ = grid_->ComputeCroppedGrid();
   set_finished(true);
 }
 

cartographer/mapping/2d/submap_2d.h

@@ -22,8 +22,8 @@
 
 #include "Eigen/Core"
 #include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/mapping/2d/grid_2d.h"
 #include "cartographer/mapping/2d/map_limits.h"
-#include "cartographer/mapping/2d/probability_grid.h"
 #include "cartographer/mapping/2d/proto/submaps_options_2d.pb.h"
 #include "cartographer/mapping/2d/range_data_inserter_2d.h"
 #include "cartographer/mapping/proto/serialization.pb.h"
@@ -36,9 +36,6 @@
 namespace cartographer {
 namespace mapping {
 
-ProbabilityGrid ComputeCroppedProbabilityGrid(
-    const ProbabilityGrid& probability_grid);
-
 proto::SubmapsOptions2D CreateSubmapsOptions2D(
     common::LuaParameterDictionary* parameter_dictionary);
 
@@ -54,7 +51,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 Grid2D* grid() const { return grid_.get(); }
 
   // Insert 'range_data' into this submap using 'range_data_inserter'. The
   // submap must not be finished yet.
@@ -63,7 +60,7 @@ class Submap2D : public Submap {
   void Finish();
 
  private:
-  std::unique_ptr<ProbabilityGrid> probability_grid_;
+  std::unique_ptr<Grid2D> grid_;
 };
 
 // Except during initialization when only a single submap exists, there are

cartographer/mapping/2d/submap_2d_test.cc

@@ -83,12 +83,12 @@ TEST(Submap2DTest, ToFromProto) {
       actual.local_pose().rotation(), 1e-6));
   EXPECT_EQ(expected.num_range_data(), actual.num_range_data());
   EXPECT_EQ(expected.finished(), actual.finished());
-  EXPECT_NEAR(expected.probability_grid().limits().resolution(),
+  EXPECT_NEAR(expected.grid()->limits().resolution(),
-              actual.probability_grid().limits().resolution(), 1e-6);
+              actual.grid()->limits().resolution(), 1e-6);
-  EXPECT_TRUE(expected.probability_grid().limits().max().isApprox(
+  EXPECT_TRUE(expected.grid()->limits().max().isApprox(
-      actual.probability_grid().limits().max(), 1e-6));
+      actual.grid()->limits().max(), 1e-6));
-  EXPECT_EQ(expected.probability_grid().limits().cell_limits().num_x_cells,
+  EXPECT_EQ(expected.grid()->limits().cell_limits().num_x_cells,
-            actual.probability_grid().limits().cell_limits().num_x_cells);
+            actual.grid()->limits().cell_limits().num_x_cells);
 }
 
 }  // namespace

cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

@@ -76,9 +76,11 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
     return nullptr;
   }
   if (options_.use_online_correlative_scan_matching()) {
+    // todo(kdaun) add CHECK on options to guarantee grid is a probability grid
     double score = real_time_correlative_scan_matcher_.Match(
         pose_prediction, filtered_gravity_aligned_point_cloud,
-        matching_submap->probability_grid(), &initial_ceres_pose);
+        *static_cast<const ProbabilityGrid*>(matching_submap->grid()),
+        &initial_ceres_pose);
     kFastCorrelativeScanMatcherScoreMetric->Observe(score);
   }
 
@@ -86,8 +88,8 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
   ceres::Solver::Summary summary;
   ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
                             filtered_gravity_aligned_point_cloud,
-                            matching_submap->probability_grid(),
+                            *matching_submap->grid(), pose_observation.get(),
-                            pose_observation.get(), &summary);
+                            &summary);
   if (pose_observation) {
     kCeresScanMatcherCostMetric->Observe(summary.final_cost);
     double residual_distance =

cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc

@@ -51,7 +51,7 @@ Eigen::Vector2d GetCornerOfFirstSubmap(
     const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data) {
   auto submap_2d = std::static_pointer_cast<const Submap2D>(
       submap_data.begin()->data.submap);
-  return submap_2d->probability_grid().limits().max();
+  return submap_2d->grid()->limits().max();
 }
 
 // Iterates over every cell in a submap, transforms the center of the cell to
@@ -68,13 +68,12 @@ std::set<SubmapId> AddSubmapsToSubmapCoverageGrid2D(
     if (freshness == submap_freshness.end()) continue;
     if (!submap.data.submap->finished()) continue;
     all_submap_ids.insert(submap.id);
-    const ProbabilityGrid& probability_grid =
+    const Grid2D& grid =
-        std::static_pointer_cast<const Submap2D>(submap.data.submap)
+        *std::static_pointer_cast<const Submap2D>(submap.data.submap)->grid();
-            ->probability_grid();
     // Iterate over every cell in a submap.
     Eigen::Array2i offset;
     CellLimits cell_limits;
-    probability_grid.ComputeCroppedLimits(&offset, &cell_limits);
+    grid.ComputeCroppedLimits(&offset, &cell_limits);
     if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) {
       LOG(WARNING) << "Empty grid found in submap ID = " << submap.id;
       continue;
@@ -83,7 +82,7 @@ std::set<SubmapId> AddSubmapsToSubmapCoverageGrid2D(
         transform::Project2D(submap.data.pose);
     for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
       const Eigen::Array2i index = xy_index + offset;
-      if (!probability_grid.IsKnown(index)) continue;
+      if (!grid.IsKnown(index)) continue;
       const transform::Rigid2d center_of_cell_in_global_frame =
           projected_submap_pose *
           transform::Rigid2d::Translation({index.x() + 0.5, index.y() + 0.5});

cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.cc

@@ -85,7 +85,7 @@ void ConstraintBuilder2D::MaybeAddConstraint(
     ++pending_computations_[current_computation_];
     const int current_computation = current_computation_;
     ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-        submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
+        submap_id, submap->grid(), [=]() EXCLUDES(mutex_) {
           ComputeConstraint(submap_id, submap, node_id,
                             false, /* match_full_submap */
                             constant_data, initial_relative_pose, constraint);
@@ -104,7 +104,7 @@ void ConstraintBuilder2D::MaybeAddGlobalConstraint(
   ++pending_computations_[current_computation_];
   const int current_computation = current_computation_;
   ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-      submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
+      submap_id, submap->grid(), [=]() EXCLUDES(mutex_) {
         ComputeConstraint(
             submap_id, submap, node_id, true, /* match_full_submap */
             constant_data, transform::Rigid2d::Identity(), constraint);

cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h

@@ -43,7 +43,7 @@ class CeresScanMatcher2D {
   CeresScanMatcher2D(const CeresScanMatcher2D&) = delete;
   CeresScanMatcher2D& operator=(const CeresScanMatcher2D&) = delete;
 
-  // Aligns 'point_cloud' within the 'probability_grid' given an
+  // Aligns 'point_cloud' within the 'grid' given an
   // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
   // 'summary'.
   void Match(const Eigen::Vector2d& target_translation,

cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h

@@ -78,7 +78,7 @@ class PrecomputationGrid2D {
  private:
   uint8 ComputeCellValue(float probability) const;
 
-  // Offset of the precomputation grid in relation to the 'probability_grid'
+  // Offset of the precomputation grid in relation to the 'grid'
   // including the additional 'width' - 1 cells.
   const Eigen::Array2i offset_;