From 4c6a2fcb287b8ae8b8e415a43aed8ff08a6bfc1c Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 24 Aug 2016 18:01:14 +0200 Subject: [PATCH] Clean up of XYIndexRangeIterator and MapLimits. (#10) --- cartographer/mapping_2d/map_limits.h | 46 ++++++------------- .../mapping_2d/probability_grid_test.cc | 3 +- .../fast_correlative_scan_matcher_test.cc | 3 +- cartographer/mapping_2d/xy_index.h | 31 ++----------- cartographer/mapping_2d/xy_index_test.cc | 6 +-- 5 files changed, 22 insertions(+), 67 deletions(-) diff --git a/cartographer/mapping_2d/map_limits.h b/cartographer/mapping_2d/map_limits.h index dd5f7b0..49173e1 100644 --- a/cartographer/mapping_2d/map_limits.h +++ b/cartographer/mapping_2d/map_limits.h @@ -38,19 +38,15 @@ namespace mapping_2d { // performance reasons. class MapLimits { public: - MapLimits(const double resolution, const Eigen::AlignedBox2d& edge_limits) { - SetLimits(resolution, edge_limits); + MapLimits(const double resolution, const Eigen::AlignedBox2d& edge_limits) + : resolution_(resolution) { + SetLimits(edge_limits); } MapLimits(const double resolution, const double max_x, const double max_y, - const CellLimits& cell_limits) { - SetLimits(resolution, max_x, max_y, cell_limits); - } - - MapLimits(const double resolution, const double center_x, - const double center_y) { - SetLimits(resolution, center_x + 100 * resolution, - center_y + 100 * resolution, CellLimits(200, 200)); + const CellLimits& cell_limits) + : resolution_(resolution) { + SetLimits(max_x, max_y, cell_limits); } // Returns the cell size in meters. All cells are square and the resolution is @@ -137,38 +133,26 @@ class MapLimits { return (std::floor(x / resolution_) + 0.5) * resolution_; } - // Sets the cell size to the specified resolution in meters and the limits of - // the grid to the specified bounding box in meters. - void SetLimits(double resolution, const Eigen::AlignedBox2d& limits) { + // Sets the limits of the grid to the specified bounding box in meters. + void SetLimits(const Eigen::AlignedBox2d& limits) { CHECK(!limits.isEmpty()); - resolution_ = resolution; const int num_x_cells = common::RoundToInt((Center(limits.max().y()) - Center(limits.min().y())) / - resolution) + + resolution_) + 1; const int num_y_cells = common::RoundToInt((Center(limits.max().x()) - Center(limits.min().x())) / - resolution) + + resolution_) + 1; - SetLimits(resolution, limits.max().x(), limits.max().y(), + SetLimits(limits.max().x(), limits.max().y(), CellLimits(num_x_cells, num_y_cells)); } - // Sets the cell size to the specified resolution in meters and the limits of - // the grid to the specified bounding box. - // - // Note that implementing this in terms of the previous SetLimits method - // results in unnecessary (and expensive?) calls to common::RoundToInt. - // - // TODO(whess): Measure whether it really is still too slow. Otherwise, - // simplify. - void SetLimits(double resolution, double max_x, double max_y, - const CellLimits& limits) { - CHECK_GT(resolution, 0.); + void SetLimits(double max_x, double max_y, const CellLimits& limits) { + CHECK_GT(resolution_, 0.); CHECK_GT(limits.num_x_cells, 0.); CHECK_GT(limits.num_y_cells, 0.); - resolution_ = resolution; cell_limits_ = limits; centered_limits_.max().x() = Center(max_x); centered_limits_.max().y() = Center(max_y); @@ -176,11 +160,7 @@ class MapLimits { resolution_ * (cell_limits_.num_y_cells - 1); centered_limits_.min().y() = centered_limits_.max().y() - resolution_ * (cell_limits_.num_x_cells - 1); - UpdateEdgeLimits(); - } - // Updates the edge limits from the previously calculated centered limits. - void UpdateEdgeLimits() { const double half_resolution = resolution_ / 2.; edge_limits_.min().x() = centered_limits_.min().x() - half_resolution; edge_limits_.min().y() = centered_limits_.min().y() - half_resolution; diff --git a/cartographer/mapping_2d/probability_grid_test.cc b/cartographer/mapping_2d/probability_grid_test.cc index fd6a385..d41e615 100644 --- a/cartographer/mapping_2d/probability_grid_test.cc +++ b/cartographer/mapping_2d/probability_grid_test.cc @@ -153,8 +153,7 @@ TEST(ProbabilityGridTest, CorrectCropping) { MapLimits(0.05, 10., 10., CellLimits(400, 400))); probability_grid.StartUpdate(); for (const Eigen::Array2i& xy_index : XYIndexRangeIterator( - probability_grid.limits().cell_limits(), Eigen::Array2i(100, 100), - Eigen::Array2i(299, 299))) { + Eigen::Array2i(100, 100), Eigen::Array2i(299, 299))) { probability_grid.SetProbability(xy_index, value_distribution(rng)); } Eigen::Array2i offset; diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc index b5c5cea..767f147 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -43,8 +43,7 @@ TEST(PrecomputationGridTest, CorrectValues) { MapLimits(0.05, 5., 5., CellLimits(250, 250))); probability_grid.StartUpdate(); for (const Eigen::Array2i& xy_index : - XYIndexRangeIterator(probability_grid.limits().cell_limits(), - Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) { + XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) { probability_grid.SetProbability( xy_index, PrecomputationGrid::ToProbability(distribution(prng))); } diff --git a/cartographer/mapping_2d/xy_index.h b/cartographer/mapping_2d/xy_index.h index 6ae3a03..e190b6e 100644 --- a/cartographer/mapping_2d/xy_index.h +++ b/cartographer/mapping_2d/xy_index.h @@ -43,15 +43,12 @@ struct CellLimits { class XYIndexRangeIterator : public std::iterator { public: - // Constructs a new iterator for the specified range. The range is cropped to - // fit within the given 'cell_limits'. - XYIndexRangeIterator(const CellLimits& cell_limits, - const Eigen::Array2i& min_xy_index, + // Constructs a new iterator for the specified range. + XYIndexRangeIterator(const Eigen::Array2i& min_xy_index, const Eigen::Array2i& max_xy_index) - : XYIndexRangeIterator( - min_xy_index.max(Eigen::Array2i::Zero()), - max_xy_index.min(Eigen::Array2i(cell_limits.num_x_cells - 1, - cell_limits.num_y_cells - 1))) {} + : min_xy_index_(min_xy_index), + max_xy_index_(max_xy_index), + xy_index_(min_xy_index) {} // Constructs a new iterator for everything contained in 'cell_limits'. explicit XYIndexRangeIterator(const CellLimits& cell_limits) @@ -59,18 +56,6 @@ class XYIndexRangeIterator Eigen::Array2i(cell_limits.num_x_cells - 1, cell_limits.num_y_cells - 1)) {} - XYIndexRangeIterator(const XYIndexRangeIterator& other) - : min_xy_index_(other.min_xy_index_), - max_xy_index_(other.max_xy_index_), - xy_index_(other.xy_index_) {} - - XYIndexRangeIterator& operator=(const XYIndexRangeIterator& other) { - min_xy_index_ = other.min_xy_index_; - max_xy_index_ = other.max_xy_index_; - xy_index_ = other.xy_index_; - return *this; - } - XYIndexRangeIterator& operator++() { // This is a necessary evil. Bounds checking is very expensive and needs to // be avoided in production. We have unit tests that exercise this check @@ -106,12 +91,6 @@ class XYIndexRangeIterator } private: - XYIndexRangeIterator(const Eigen::Array2i& min_xy_index, - const Eigen::Array2i& max_xy_index) - : min_xy_index_(min_xy_index), - max_xy_index_(max_xy_index), - xy_index_(min_xy_index) {} - Eigen::Array2i min_xy_index_; Eigen::Array2i max_xy_index_; Eigen::Array2i xy_index_; diff --git a/cartographer/mapping_2d/xy_index_test.cc b/cartographer/mapping_2d/xy_index_test.cc index 8193530..7877ebe 100644 --- a/cartographer/mapping_2d/xy_index_test.cc +++ b/cartographer/mapping_2d/xy_index_test.cc @@ -23,16 +23,14 @@ namespace mapping_2d { namespace { TEST(XYIndexTest, XYIndexRangeIterator) { - const CellLimits limits(5, 5); const Eigen::Array2i min(1, 2); const Eigen::Array2i max(3, 4); - XYIndexRangeIterator it(limits, min, max); + XYIndexRangeIterator it(min, max); EXPECT_TRUE((min == *it.begin()).all()) << *it.begin(); EXPECT_TRUE((Eigen::Array2i(1, 5) == *it.end()).all()) << *it.end(); EXPECT_TRUE((min == *it).all()) << *it; int num_indices = 0; - for (const Eigen::Array2i& xy_index : - XYIndexRangeIterator(limits, min, max)) { + for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(min, max)) { LOG(INFO) << xy_index; EXPECT_TRUE((xy_index >= min).all()); EXPECT_TRUE((xy_index <= max).all());