12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Clean up xy_index.h. (#9) 

Removes and simplifies code from free standing functions in xy_index.h.
master
Wolfgang Hess 2016-08-24 15:33:13 +02:00 committed by GitHub
parent 1be7e8e226
commit afa0cef31d
6 changed files with 39 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
README.md
View File

@ -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 \

37
cartographer/mapping_2d/map_limits.h
View File

@ -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() -

12
cartographer/mapping_2d/probability_grid.h
View File

@ -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) {

20
cartographer/mapping_2d/probability_grid_test.cc
View File

@ -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);

27
cartographer/mapping_2d/ray_casting.cc
View File

@ -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<void(const Eigen::Array2i&)>& hit_visitor,
const std::function<void(const Eigen::Array2i&)>& 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<Eigen::Array2i> 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);
}
}

25
cartographer/mapping_2d/xy_index.h
View File

@ -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