Grow the ProbabilityGrid as needed instead of preconfiguring its size. (#387)

This removes the 'half_length' option.

PAIR=wohe

cartographer/mapping_2d/proto/submaps_options.proto

@@ -22,9 +22,6 @@ message SubmapsOptions {
   // Resolution of the map in meters.
   optional double resolution = 1;
 
-  // Half the width/height of each submap, its "radius".
-  optional double half_length = 2;
-
   // Number of scans before adding a new submap. Each submap will get twice the
   // number of scans inserted: First for initialization without being matched
   // against, then while being matched.

cartographer/mapping_2d/ray_casting.cc

@@ -146,6 +146,21 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
   CHECK_EQ(current.y(), end.y() / kSubpixelScale);
 }
 
+void GrowAsNeeded(const sensor::RangeData& range_data, ProbabilityGrid* const probability_grid) {
+  Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
+  constexpr float kPadding = 1e-6f;
+  for (const Eigen::Vector3f& hit : range_data.returns) {
+    bounding_box.extend(hit.head<2>());
+  }
+  for (const Eigen::Vector3f& miss : range_data.misses) {
+    bounding_box.extend(miss.head<2>());
+  }
+  probability_grid->GrowLimits(bounding_box.min().x() - kPadding,
+                               bounding_box.min().y() - kPadding);
+  probability_grid->GrowLimits(bounding_box.max().x() + kPadding,
+                               bounding_box.max().y() + kPadding);
+}
+
 }  // namespace
 
 void CastRays(const sensor::RangeData& range_data,
@@ -153,6 +168,8 @@ void CastRays(const sensor::RangeData& range_data,
               const std::vector<uint16>& miss_table,
               const bool insert_free_space,
               ProbabilityGrid* const probability_grid) {
+  GrowAsNeeded(range_data, probability_grid);
+
   const MapLimits& limits = probability_grid->limits();
   const double superscaled_resolution = limits.resolution() / kSubpixelScale;
   const MapLimits superscaled_limits(

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -51,7 +51,6 @@ class SparsePoseGraphTest : public ::testing::Test {
       auto parameter_dictionary = common::MakeDictionary(R"text(
           return {
             resolution = 0.05,
-            half_length = 21.,
             num_range_data = 1,
             range_data_inserter = {
               insert_free_space = true,

cartographer/mapping_2d/submaps.cc

@@ -54,7 +54,6 @@ proto::SubmapsOptions CreateSubmapsOptions(
     common::LuaParameterDictionary* const parameter_dictionary) {
   proto::SubmapsOptions options;
   options.set_resolution(parameter_dictionary->GetDouble("resolution"));
-  options.set_half_length(parameter_dictionary->GetDouble("half_length"));
   options.set_num_range_data(
       parameter_dictionary->GetNonNegativeInt("num_range_data"));
   *options.mutable_range_data_inserter_options() =
@@ -179,14 +178,13 @@ void ActiveSubmaps::AddSubmap(const Eigen::Vector2f& origin) {
     // reduce peak memory usage a bit.
     FinishSubmap();
   }
-  const int num_cells_per_dimension =
+  constexpr int kInitialSubmapSize = 100;
-      common::RoundToInt(2. * options_.half_length() / options_.resolution()) +
-      1;
   submaps_.push_back(common::make_unique<Submap>(
       MapLimits(options_.resolution(),
-                origin.cast<double>() +
+                origin.cast<double>() + 0.5 * kInitialSubmapSize *
-                    options_.half_length() * Eigen::Vector2d::Ones(),
+                                            options_.resolution() *
-                CellLimits(num_cells_per_dimension, num_cells_per_dimension)),
+                                            Eigen::Vector2d::Ones(),
+                CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
       origin));
   LOG(INFO) << "Added submap " << matching_submap_index_ + submaps_.size();
 }

cartographer/mapping_2d/submaps_test.cc

@@ -36,7 +36,6 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) {
   auto parameter_dictionary = common::MakeDictionary(
       "return {"
       "resolution = 0.05, "
-      "half_length = 10., "
       "num_range_data = " +
       std::to_string(kNumRangeData) +
       ", "

configuration_files/trajectory_builder_2d.lua

@@ -57,7 +57,6 @@ TRAJECTORY_BUILDER_2D = {
 
   submaps = {
     resolution = 0.05,
-    half_length = 200.,
     num_range_data = 90,
     range_data_inserter = {
       insert_free_space = true,

docs/source/configuration.rst

@@ -242,9 +242,6 @@ cartographer.mapping_2d.proto.SubmapsOptions
 double resolution
   Resolution of the map in meters.
 
-double half_length
-  Half the width/height of each submap, its "radius".
-
 int32 num_range_data
   Number of scans before adding a new submap. Each submap will get twice the
   number of scans inserted: First for initialization without being matched