From b0b4f30007e8472d0b4942e18bf5400e99e166dc Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 10 Jul 2017 15:01:13 +0200 Subject: [PATCH] Cleanup of cell index conversion in 2D. (#399) This makes 2D more similar to 3D. GetProbability() only takes cell indices which are computed using GetCellIndex(). --- cartographer/mapping_2d/map_limits.h | 21 ++++--- cartographer/mapping_2d/probability_grid.h | 58 +++++++++---------- .../mapping_2d/probability_grid_test.cc | 39 +++++++------ .../mapping_2d/range_data_inserter_test.cc | 50 ++++++++++------ cartographer/mapping_2d/ray_casting.cc | 18 +++--- .../scan_matching/ceres_scan_matcher_test.cc | 2 +- .../scan_matching/correlative_scan_matcher.cc | 3 +- cartographer/sensor/range_data.cc | 6 +- 8 files changed, 101 insertions(+), 96 deletions(-) diff --git a/cartographer/mapping_2d/map_limits.h b/cartographer/mapping_2d/map_limits.h index bb33973..8f4a033 100644 --- a/cartographer/mapping_2d/map_limits.h +++ b/cartographer/mapping_2d/map_limits.h @@ -63,23 +63,22 @@ class MapLimits { // Returns the limits of the grid in number of cells. const CellLimits& cell_limits() const { return cell_limits_; } - // Returns the index of the cell containing the point ('x', 'y') which may be - // outside the map, i.e., negative or too large indices that will return - // false for Contains(). - Eigen::Array2i GetXYIndexOfCellContainingPoint(const double x, - const double y) const { + // Returns the index of the cell containing the 'point' which may be outside + // the map, i.e., negative or too large indices that will return false for + // Contains(). + Eigen::Array2i GetCellIndex(const Eigen::Vector2f& point) const { // Index values are row major and the top left has Eigen::Array2i::Zero() // and contains (centered_max_x, centered_max_y). We need to flip and // rotate. return Eigen::Array2i( - common::RoundToInt((max_.y() - y) / resolution_ - 0.5), - common::RoundToInt((max_.x() - x) / resolution_ - 0.5)); + common::RoundToInt((max_.y() - point.y()) / resolution_ - 0.5), + common::RoundToInt((max_.x() - point.x()) / resolution_ - 0.5)); } - // Returns true of the ProbabilityGrid contains 'xy_index'. - bool Contains(const Eigen::Array2i& xy_index) const { - return (Eigen::Array2i(0, 0) <= xy_index).all() && - (xy_index < + // Returns true if the ProbabilityGrid contains 'cell_index'. + bool Contains(const Eigen::Array2i& cell_index) const { + return (Eigen::Array2i(0, 0) <= cell_index).all() && + (cell_index < Eigen::Array2i(cell_limits_.num_x_cells, cell_limits_.num_y_cells)) .all(); } diff --git a/cartographer/mapping_2d/probability_grid.h b/cartographer/mapping_2d/probability_grid.h index 6074144..f5f0b77 100644 --- a/cartographer/mapping_2d/probability_grid.h +++ b/cartographer/mapping_2d/probability_grid.h @@ -74,54 +74,50 @@ class ProbabilityGrid { } } - // Sets the probability of the cell at 'xy_index' to the given 'probability'. - // Only allowed if the cell was unknown before. - void SetProbability(const Eigen::Array2i& xy_index, const float probability) { - uint16& cell = cells_[GetIndexOfCell(xy_index)]; + // Sets the probability of the cell at 'cell_index' to the given + // 'probability'. Only allowed if the cell was unknown before. + void SetProbability(const Eigen::Array2i& cell_index, + const float probability) { + uint16& cell = cells_[ToFlatIndex(cell_index)]; CHECK_EQ(cell, mapping::kUnknownProbabilityValue); cell = mapping::ProbabilityToValue(probability); - known_cells_box_.extend(xy_index.matrix()); + known_cells_box_.extend(cell_index.matrix()); } // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds() - // to the probability of the cell at 'xy_index' if the cell has not already + // to the probability of the cell at 'cell_index' if the cell has not already // been updated. Multiple updates of the same cell will be ignored until // StartUpdate() is called. Returns true if the cell was updated. // // If this is the first call to ApplyOdds() for the specified cell, its value // will be set to probability corresponding to 'odds'. - bool ApplyLookupTable(const Eigen::Array2i& xy_index, + bool ApplyLookupTable(const Eigen::Array2i& cell_index, const std::vector& table) { DCHECK_EQ(table.size(), mapping::kUpdateMarker); - const int cell_index = GetIndexOfCell(xy_index); - uint16& cell = cells_[cell_index]; + const int flat_index = ToFlatIndex(cell_index); + uint16& cell = cells_[flat_index]; if (cell >= mapping::kUpdateMarker) { return false; } - update_indices_.push_back(cell_index); + update_indices_.push_back(flat_index); cell = table[cell]; DCHECK_GE(cell, mapping::kUpdateMarker); - known_cells_box_.extend(xy_index.matrix()); + known_cells_box_.extend(cell_index.matrix()); return true; } - // Returns the probability of the cell with 'xy_index'. - float GetProbability(const Eigen::Array2i& xy_index) const { - if (limits_.Contains(xy_index)) { - return mapping::ValueToProbability(cells_[GetIndexOfCell(xy_index)]); + // Returns the probability of the cell with 'cell_index'. + float GetProbability(const Eigen::Array2i& cell_index) const { + if (limits_.Contains(cell_index)) { + return mapping::ValueToProbability(cells_[ToFlatIndex(cell_index)]); } return mapping::kMinProbability; } - // Returns the probability of the cell containing the point ('x', 'y'). - float GetProbability(const double x, const double y) const { - return GetProbability(limits_.GetXYIndexOfCellContainingPoint(x, y)); - } - // Returns true if the probability at the specified index is known. - bool IsKnown(const Eigen::Array2i& xy_index) const { - return limits_.Contains(xy_index) && cells_[GetIndexOfCell(xy_index)] != - mapping::kUnknownProbabilityValue; + bool IsKnown(const Eigen::Array2i& cell_index) const { + return limits_.Contains(cell_index) && + cells_[ToFlatIndex(cell_index)] != mapping::kUnknownProbabilityValue; } // Fills in 'offset' and 'limits' to define a subregion of that contains all @@ -138,12 +134,12 @@ class ProbabilityGrid { } } - // Grows the map as necessary to include 'x' and 'y'. This changes the meaning - // of these coordinates going forward. This method must be called immediately + // Grows the map as necessary to include 'point'. This changes the meaning of + // these coordinates going forward. This method must be called immediately // after 'StartUpdate', before any calls to 'ApplyLookupTable'. - void GrowLimits(const double x, const double y) { + void GrowLimits(const Eigen::Vector2f& point) { CHECK(update_indices_.empty()); - while (!limits_.Contains(limits_.GetXYIndexOfCellContainingPoint(x, y))) { + while (!limits_.Contains(limits_.GetCellIndex(point))) { const int x_offset = limits_.cell_limits().num_x_cells / 2; const int y_offset = limits_.cell_limits().num_y_cells / 2; const MapLimits new_limits( @@ -193,10 +189,10 @@ class ProbabilityGrid { } private: - // Returns the index of the cell at 'xy_index'. - int GetIndexOfCell(const Eigen::Array2i& xy_index) const { - CHECK(limits_.Contains(xy_index)) << xy_index; - return limits_.cell_limits().num_x_cells * xy_index.y() + xy_index.x(); + // Converts a 'cell_index' into an index into 'cells_'. + int ToFlatIndex(const Eigen::Array2i& cell_index) const { + CHECK(limits_.Contains(cell_index)) << cell_index; + return limits_.cell_limits().num_x_cells * cell_index.y() + cell_index.x(); } MapLimits limits_; diff --git a/cartographer/mapping_2d/probability_grid_test.cc b/cartographer/mapping_2d/probability_grid_test.cc index 7c674fd..74a0da5 100644 --- a/cartographer/mapping_2d/probability_grid_test.cc +++ b/cartographer/mapping_2d/probability_grid_test.cc @@ -125,20 +125,21 @@ TEST(ProbabilityGridTest, GetProbability) { probability_grid.StartUpdate(); probability_grid.SetProbability( - limits.GetXYIndexOfCellContainingPoint(-0.5, 0.5), + limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f)), mapping::kMaxProbability); - EXPECT_NEAR(probability_grid.GetProbability(-0.5, 0.5), + EXPECT_NEAR(probability_grid.GetProbability( + limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f))), mapping::kMaxProbability, 1e-6); for (const Eigen::Array2i& xy_index : - {limits.GetXYIndexOfCellContainingPoint(-0.5, 1.5), - limits.GetXYIndexOfCellContainingPoint(0.5, 0.5), - limits.GetXYIndexOfCellContainingPoint(0.5, 1.5)}) { + {limits.GetCellIndex(Eigen::Vector2f(-0.5f, 1.5f)), + limits.GetCellIndex(Eigen::Vector2f(0.5f, 0.5f)), + limits.GetCellIndex(Eigen::Vector2f(0.5f, 1.5f))}) { EXPECT_TRUE(limits.Contains(xy_index)); EXPECT_FALSE(probability_grid.IsKnown(xy_index)); } } -TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) { +TEST(ProbabilityGridTest, GetCellIndex) { ProbabilityGrid probability_grid( MapLimits(2., Eigen::Vector2d(8., 14.), CellLimits(14, 8))); @@ -147,33 +148,33 @@ TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) { ASSERT_EQ(14, cell_limits.num_x_cells); ASSERT_EQ(8, cell_limits.num_y_cells); EXPECT_TRUE( - (Eigen::Array2i(0, 0) == limits.GetXYIndexOfCellContainingPoint(7, 13)) + (Eigen::Array2i(0, 0) == limits.GetCellIndex(Eigen::Vector2f(7.f, 13.f))) .all()); + EXPECT_TRUE((Eigen::Array2i(13, 0) == + limits.GetCellIndex(Eigen::Vector2f(7.f, -13.f))) + .all()); EXPECT_TRUE( - (Eigen::Array2i(13, 0) == limits.GetXYIndexOfCellContainingPoint(7, -13)) - .all()); - EXPECT_TRUE( - (Eigen::Array2i(0, 7) == limits.GetXYIndexOfCellContainingPoint(-7, 13)) - .all()); - EXPECT_TRUE( - (Eigen::Array2i(13, 7) == limits.GetXYIndexOfCellContainingPoint(-7, -13)) + (Eigen::Array2i(0, 7) == limits.GetCellIndex(Eigen::Vector2f(-7.f, 13.f))) .all()); + EXPECT_TRUE((Eigen::Array2i(13, 7) == + limits.GetCellIndex(Eigen::Vector2f(-7.f, -13.f))) + .all()); // Check around the origin. EXPECT_TRUE( - (Eigen::Array2i(6, 3) == limits.GetXYIndexOfCellContainingPoint(0.5, 0.5)) + (Eigen::Array2i(6, 3) == limits.GetCellIndex(Eigen::Vector2f(0.5f, 0.5f))) .all()); EXPECT_TRUE( - (Eigen::Array2i(6, 3) == limits.GetXYIndexOfCellContainingPoint(1.5, 1.5)) + (Eigen::Array2i(6, 3) == limits.GetCellIndex(Eigen::Vector2f(1.5f, 1.5f))) .all()); EXPECT_TRUE((Eigen::Array2i(7, 3) == - limits.GetXYIndexOfCellContainingPoint(0.5, -0.5)) + limits.GetCellIndex(Eigen::Vector2f(0.5f, -0.5f))) .all()); EXPECT_TRUE((Eigen::Array2i(6, 4) == - limits.GetXYIndexOfCellContainingPoint(-0.5, 0.5)) + limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f))) .all()); EXPECT_TRUE((Eigen::Array2i(7, 4) == - limits.GetXYIndexOfCellContainingPoint(-0.5, -0.5)) + limits.GetCellIndex(Eigen::Vector2f(-0.5f, -0.5f))) .all()); } diff --git a/cartographer/mapping_2d/range_data_inserter_test.cc b/cartographer/mapping_2d/range_data_inserter_test.cc index 7add520..baf5702 100644 --- a/cartographer/mapping_2d/range_data_inserter_test.cc +++ b/cartographer/mapping_2d/range_data_inserter_test.cc @@ -45,12 +45,12 @@ class RangeDataInserterTest : public ::testing::Test { void InsertPointCloud() { sensor::RangeData range_data; - range_data.returns.emplace_back(-3.5, 0.5, 0.f); - range_data.returns.emplace_back(-2.5, 1.5, 0.f); - range_data.returns.emplace_back(-1.5, 2.5, 0.f); - range_data.returns.emplace_back(-0.5, 3.5, 0.f); - range_data.origin.x() = -0.5; - range_data.origin.y() = 0.5; + range_data.returns.emplace_back(-3.5f, 0.5f, 0.f); + range_data.returns.emplace_back(-2.5f, 1.5f, 0.f); + range_data.returns.emplace_back(-1.5f, 2.5f, 0.f); + range_data.returns.emplace_back(-0.5f, 3.5f, 0.f); + range_data.origin.x() = -0.5f; + range_data.origin.y() = 0.5f; probability_grid_.StartUpdate(); range_data_inserter_->Insert(range_data, &probability_grid_); } @@ -81,19 +81,19 @@ TEST_F(RangeDataInserterTest, InsertPointCloud) { State::HIT}}; for (int row = 0; row != 5; ++row) { for (int column = 0; column != 5; ++column) { - Eigen::Array2i xy_index(row, column); - EXPECT_TRUE(probability_grid_.limits().Contains(xy_index)); + Eigen::Array2i cell_index(row, column); + EXPECT_TRUE(probability_grid_.limits().Contains(cell_index)); switch (expected_states[column][row]) { case State::UNKNOWN: - EXPECT_FALSE(probability_grid_.IsKnown(xy_index)); + EXPECT_FALSE(probability_grid_.IsKnown(cell_index)); break; case State::MISS: EXPECT_NEAR(options_.miss_probability(), - probability_grid_.GetProbability(xy_index), 1e-4); + probability_grid_.GetProbability(cell_index), 1e-4); break; case State::HIT: EXPECT_NEAR(options_.hit_probability(), - probability_grid_.GetProbability(xy_index), 1e-4); + probability_grid_.GetProbability(cell_index), 1e-4); break; } } @@ -102,18 +102,30 @@ TEST_F(RangeDataInserterTest, InsertPointCloud) { TEST_F(RangeDataInserterTest, ProbabilityProgression) { InsertPointCloud(); - EXPECT_NEAR(options_.hit_probability(), - probability_grid_.GetProbability(-3.5, 0.5), 1e-4); - EXPECT_NEAR(options_.miss_probability(), - probability_grid_.GetProbability(-2.5, 0.5), 1e-4); + EXPECT_NEAR( + options_.hit_probability(), + probability_grid_.GetProbability(probability_grid_.limits().GetCellIndex( + Eigen::Vector2f(-3.5f, 0.5f))), + 1e-4); + EXPECT_NEAR( + options_.miss_probability(), + probability_grid_.GetProbability(probability_grid_.limits().GetCellIndex( + Eigen::Vector2f(-2.5f, 0.5f))), + 1e-4); for (int i = 0; i < 1000; ++i) { InsertPointCloud(); } - EXPECT_NEAR(mapping::kMaxProbability, - probability_grid_.GetProbability(-3.5, 0.5), 1e-3); - EXPECT_NEAR(mapping::kMinProbability, - probability_grid_.GetProbability(-2.5, 0.5), 1e-3); + EXPECT_NEAR( + mapping::kMaxProbability, + probability_grid_.GetProbability(probability_grid_.limits().GetCellIndex( + Eigen::Vector2f(-3.5f, 0.5f))), + 1e-3); + EXPECT_NEAR( + mapping::kMinProbability, + probability_grid_.GetProbability(probability_grid_.limits().GetCellIndex( + Eigen::Vector2f(-2.5f, 0.5f))), + 1e-3); } } // namespace diff --git a/cartographer/mapping_2d/ray_casting.cc b/cartographer/mapping_2d/ray_casting.cc index e8b6225..7a82929 100644 --- a/cartographer/mapping_2d/ray_casting.cc +++ b/cartographer/mapping_2d/ray_casting.cc @@ -156,10 +156,10 @@ void GrowAsNeeded(const sensor::RangeData& range_data, 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); + probability_grid->GrowLimits(bounding_box.min() - + kPadding * Eigen::Vector2f::Ones()); + probability_grid->GrowLimits(bounding_box.max() + + kPadding * Eigen::Vector2f::Ones()); } } // namespace @@ -178,14 +178,12 @@ void CastRays(const sensor::RangeData& range_data, CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale, limits.cell_limits().num_y_cells * kSubpixelScale)); const Eigen::Array2i begin = - superscaled_limits.GetXYIndexOfCellContainingPoint(range_data.origin.x(), - range_data.origin.y()); + superscaled_limits.GetCellIndex(range_data.origin.head<2>()); // Compute and add the end points. std::vector ends; ends.reserve(range_data.returns.size()); for (const Eigen::Vector3f& hit : range_data.returns) { - ends.push_back( - superscaled_limits.GetXYIndexOfCellContainingPoint(hit.x(), hit.y())); + ends.push_back(superscaled_limits.GetCellIndex(hit.head<2>())); probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table); } @@ -200,9 +198,7 @@ void CastRays(const sensor::RangeData& range_data, // Finally, compute and add empty rays based on misses in the scan. for (const Eigen::Vector3f& missing_echo : range_data.misses) { - CastRay(begin, - superscaled_limits.GetXYIndexOfCellContainingPoint( - missing_echo.x(), missing_echo.y()), + CastRay(begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()), miss_table, probability_grid); } } diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc index 495ea4f..03a5730 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc @@ -37,7 +37,7 @@ class CeresScanMatcherTest : public ::testing::Test { : probability_grid_( MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20))) { probability_grid_.SetProbability( - probability_grid_.limits().GetXYIndexOfCellContainingPoint(-3.5, 2.5), + probability_grid_.limits().GetCellIndex(Eigen::Vector2f(-3.5f, 2.5f)), mapping::kMaxProbability); point_cloud_.emplace_back(-3.f, 2.f, 0.f); diff --git a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc index 10fa355..2f6cda4 100644 --- a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc @@ -120,8 +120,7 @@ std::vector DiscretizeScans( const Eigen::Vector2f translated_point = Eigen::Affine2f(initial_translation) * point.head<2>(); discrete_scans.back().push_back( - map_limits.GetXYIndexOfCellContainingPoint(translated_point.x(), - translated_point.y())); + map_limits.GetCellIndex(translated_point)); } } return discrete_scans; diff --git a/cartographer/sensor/range_data.cc b/cartographer/sensor/range_data.cc index 69c7d58..9c2a94b 100644 --- a/cartographer/sensor/range_data.cc +++ b/cartographer/sensor/range_data.cc @@ -48,14 +48,16 @@ proto::CompressedRangeData ToProto( CompressedRangeData FromProto(const proto::CompressedRangeData& proto) { return CompressedRangeData{ - transform::ToEigen(proto.origin()), CompressedPointCloud(proto.returns()), + transform::ToEigen(proto.origin()), + CompressedPointCloud(proto.returns()), CompressedPointCloud(proto.misses()), }; } CompressedRangeData Compress(const RangeData& range_data) { return CompressedRangeData{ - range_data.origin, CompressedPointCloud(range_data.returns), + range_data.origin, + CompressedPointCloud(range_data.returns), CompressedPointCloud(range_data.misses), }; }