diff --git a/README.md b/README.md index 417aa1d..25211f0 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,7 @@ See https://github.com/googlecartographer/cartographer_ros ## Installation without ROS -On Ubuntu 14.04 (Trusty): +On Ubuntu 14.04 (Trusty) or Ubuntu 16.04 (Xenial): # Install the required libraries that are available as debs sudo apt-get install \ diff --git a/cartographer/mapping_2d/map_limits.h b/cartographer/mapping_2d/map_limits.h index 04bdc7f..dd5f7b0 100644 --- a/cartographer/mapping_2d/map_limits.h +++ b/cartographer/mapping_2d/map_limits.h @@ -74,9 +74,12 @@ class MapLimits { // false for Contains(). Eigen::Array2i GetXYIndexOfCellContainingPoint(const double x, const double y) const { - return mapping_2d::GetXYIndexOfCellContainingPoint( - x, y, centered_limits_.max().x(), centered_limits_.max().y(), - resolution_); + // 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((centered_limits_.max().y() - y) / resolution_), + common::RoundToInt((centered_limits_.max().x() - x) / resolution_)); } // Returns true of the ProbabilityGrid contains 'xy_index'. @@ -129,20 +132,24 @@ class MapLimits { } private: + // Returns the center of the resolution interval containing 'x'. + double Center(const double x) { + 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) { CHECK(!limits.isEmpty()); - const int num_x_cells = - common::RoundToInt((mapping_2d::Center(limits.max().y(), resolution) - - mapping_2d::Center(limits.min().y(), resolution)) / - resolution) + - 1; - const int num_y_cells = - common::RoundToInt((mapping_2d::Center(limits.max().x(), resolution) - - mapping_2d::Center(limits.min().x(), resolution)) / - resolution) + - 1; + resolution_ = resolution; + const int num_x_cells = common::RoundToInt((Center(limits.max().y()) - + Center(limits.min().y())) / + resolution) + + 1; + const int num_y_cells = common::RoundToInt((Center(limits.max().x()) - + Center(limits.min().x())) / + resolution) + + 1; SetLimits(resolution, limits.max().x(), limits.max().y(), CellLimits(num_x_cells, num_y_cells)); } @@ -163,8 +170,8 @@ class MapLimits { resolution_ = resolution; cell_limits_ = limits; - centered_limits_.max().x() = Center(max_x, resolution); - centered_limits_.max().y() = Center(max_y, resolution); + centered_limits_.max().x() = Center(max_x); + centered_limits_.max().y() = Center(max_y); centered_limits_.min().x() = centered_limits_.max().x() - resolution_ * (cell_limits_.num_y_cells - 1); centered_limits_.min().y() = centered_limits_.max().y() - diff --git a/cartographer/mapping_2d/probability_grid.h b/cartographer/mapping_2d/probability_grid.h index 7bb9d03..ea3e6eb 100644 --- a/cartographer/mapping_2d/probability_grid.h +++ b/cartographer/mapping_2d/probability_grid.h @@ -111,15 +111,6 @@ class ProbabilityGrid { mapping::kUnknownProbabilityValue; } - // Returns the center of the cell at 'xy_index'. - void GetCenterOfCellWithXYIndex(const Eigen::Array2i& xy_index, - double* const x, double* const y) const { - *CHECK_NOTNULL(x) = limits_.edge_limits().max().x() - - (xy_index.y() + 0.5) * limits_.resolution(); - *CHECK_NOTNULL(y) = limits_.edge_limits().max().y() - - (xy_index.x() + 0.5) * limits_.resolution(); - } - // Fills in 'offset' and 'limits' to define a subregion of that contains all // known cells. void ComputeCroppedLimits(Eigen::Array2i* const offset, @@ -168,8 +159,7 @@ class ProbabilityGrid { // 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 mapping_2d::GetIndexOfCell(xy_index, - limits_.cell_limits().num_x_cells); + return limits_.cell_limits().num_x_cells * xy_index.y() + xy_index.x(); } void UpdateBounds(const Eigen::Array2i& xy_index) { diff --git a/cartographer/mapping_2d/probability_grid_test.cc b/cartographer/mapping_2d/probability_grid_test.cc index b5abb5e..fd6a385 100644 --- a/cartographer/mapping_2d/probability_grid_test.cc +++ b/cartographer/mapping_2d/probability_grid_test.cc @@ -144,26 +144,6 @@ TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) { .all()); } -TEST(ProbabilityGridTest, GetCenterOfCell) { - ProbabilityGrid probability_grid(MapLimits( - 2., - Eigen::AlignedBox2d(Eigen::Vector2d(-4., 0.), Eigen::Vector2d(-0., 6.)))); - - // Limits are on the edge of a cell. Make sure that the grid grows (in - // positive direction). - const MapLimits& limits = probability_grid.limits(); - const CellLimits& cell_limits = limits.cell_limits(); - EXPECT_EQ(4, cell_limits.num_x_cells); - EXPECT_EQ(3, cell_limits.num_y_cells); - - const Eigen::Array2i xy_index(3, 2); - double x, y; - probability_grid.GetCenterOfCellWithXYIndex(xy_index, &x, &y); - EXPECT_NEAR(-3., x, 1e-6); - EXPECT_NEAR(1., y, 1e-6); - EXPECT_TRUE((xy_index == limits.GetXYIndexOfCellContainingPoint(x, y)).all()); -} - TEST(ProbabilityGridTest, CorrectCropping) { // Create a probability grid with random values. std::mt19937 rng(42); diff --git a/cartographer/mapping_2d/ray_casting.cc b/cartographer/mapping_2d/ray_casting.cc index 10dab95..04b4451 100644 --- a/cartographer/mapping_2d/ray_casting.cc +++ b/cartographer/mapping_2d/ray_casting.cc @@ -150,20 +150,25 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end, void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits, const std::function& hit_visitor, const std::function& miss_visitor) { - const double resolution = limits.resolution() / kSubpixelScale; + const double superscaled_resolution = limits.resolution() / kSubpixelScale; const Eigen::Vector2d superscaled_centered_max = - limits.edge_limits().max() - resolution / 2. * Eigen::Vector2d::Ones(); - const Eigen::Array2i begin = GetXYIndexOfCellContainingPoint( - laser_fan.origin.x(), laser_fan.origin.y(), superscaled_centered_max.x(), - superscaled_centered_max.y(), resolution); + limits.edge_limits().max() - + superscaled_resolution / 2. * Eigen::Vector2d::Ones(); + const MapLimits superscaled_limits( + superscaled_resolution, superscaled_centered_max.x(), + superscaled_centered_max.y(), + CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale, + limits.cell_limits().num_y_cells * kSubpixelScale)); + const Eigen::Array2i begin = + superscaled_limits.GetXYIndexOfCellContainingPoint(laser_fan.origin.x(), + laser_fan.origin.y()); // Compute and add the end points. std::vector ends; ends.reserve(laser_fan.point_cloud.size()); for (const Eigen::Vector2f& laser_return : laser_fan.point_cloud) { - ends.push_back(GetXYIndexOfCellContainingPoint( - laser_return.x(), laser_return.y(), superscaled_centered_max.x(), - superscaled_centered_max.y(), resolution)); + ends.push_back(superscaled_limits.GetXYIndexOfCellContainingPoint( + laser_return.x(), laser_return.y())); hit_visitor(ends.back() / kSubpixelScale); } @@ -175,10 +180,8 @@ void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits, // Finally, compute and add empty rays based on missing echos in the scan. for (const Eigen::Vector2f& missing_echo : laser_fan.missing_echo_point_cloud) { - CastRay(begin, GetXYIndexOfCellContainingPoint( - missing_echo.x(), missing_echo.y(), - superscaled_centered_max.x(), - superscaled_centered_max.y(), resolution), + CastRay(begin, superscaled_limits.GetXYIndexOfCellContainingPoint( + missing_echo.x(), missing_echo.y()), miss_visitor); } } diff --git a/cartographer/mapping_2d/xy_index.h b/cartographer/mapping_2d/xy_index.h index 4806fa4..6ae3a03 100644 --- a/cartographer/mapping_2d/xy_index.h +++ b/cartographer/mapping_2d/xy_index.h @@ -117,31 +117,6 @@ class XYIndexRangeIterator Eigen::Array2i xy_index_; }; -// Returns the center of the resolution interval containing x. -inline double Center(double x, double resolution) { - return (std::floor(x / resolution) + 0.5) * resolution; -} - -// Returns the index of the cell containing the point (x, y) which is allowed to -// lie outside the map. -inline Eigen::Array2i GetXYIndexOfCellContainingPoint(double x, double y, - double centered_max_x, - double centered_max_y, - double resolution) { - // 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. - const int cell_x = common::RoundToInt((centered_max_y - y) / resolution); - const int cell_y = common::RoundToInt((centered_max_x - x) / resolution); - return Eigen::Array2i(cell_x, cell_y); -} - -// Returns the index into the vector with 'stride' for the cell with 'xy_index'. -inline int GetIndexOfCell(const Eigen::Array2i& xy_index, int stride) { - CHECK_GE(xy_index.x(), 0); - CHECK_GE(xy_index.y(), 0); - return stride * xy_index.y() + xy_index.x(); -} - } // namespace mapping_2d } // namespace cartographer