diff --git a/cartographer/mapping_2d/proto/submaps_options.proto b/cartographer/mapping_2d/proto/submaps_options.proto index 5538c6a..9e24649 100644 --- a/cartographer/mapping_2d/proto/submaps_options.proto +++ b/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. diff --git a/cartographer/mapping_2d/ray_casting.cc b/cartographer/mapping_2d/ray_casting.cc index fcada10..e3efb2a 100644 --- a/cartographer/mapping_2d/ray_casting.cc +++ b/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& 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( diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index 77a9763..aec849c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/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, diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc index d727906..f45b77a 100644 --- a/cartographer/mapping_2d/submaps.cc +++ b/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 = - common::RoundToInt(2. * options_.half_length() / options_.resolution()) + - 1; + constexpr int kInitialSubmapSize = 100; submaps_.push_back(common::make_unique( MapLimits(options_.resolution(), - origin.cast() + - options_.half_length() * Eigen::Vector2d::Ones(), - CellLimits(num_cells_per_dimension, num_cells_per_dimension)), + origin.cast() + 0.5 * kInitialSubmapSize * + options_.resolution() * + Eigen::Vector2d::Ones(), + CellLimits(kInitialSubmapSize, kInitialSubmapSize)), origin)); LOG(INFO) << "Added submap " << matching_submap_index_ + submaps_.size(); } diff --git a/cartographer/mapping_2d/submaps_test.cc b/cartographer/mapping_2d/submaps_test.cc index 16b0628..682ddf7 100644 --- a/cartographer/mapping_2d/submaps_test.cc +++ b/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) + ", " diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index 8b06f50..a61835c 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/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, diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index 694383d..cb417b3 100644 --- a/docs/source/configuration.rst +++ b/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