Clean up xy_index.h. (#9)
Removes and simplifies code from free standing functions in xy_index.h.master
parent
1be7e8e226
commit
afa0cef31d
|
@ -11,7 +11,7 @@ See https://github.com/googlecartographer/cartographer_ros
|
||||||
|
|
||||||
## Installation without 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
|
# Install the required libraries that are available as debs
|
||||||
sudo apt-get install \
|
sudo apt-get install \
|
||||||
|
|
|
@ -74,9 +74,12 @@ class MapLimits {
|
||||||
// false for Contains().
|
// false for Contains().
|
||||||
Eigen::Array2i GetXYIndexOfCellContainingPoint(const double x,
|
Eigen::Array2i GetXYIndexOfCellContainingPoint(const double x,
|
||||||
const double y) const {
|
const double y) const {
|
||||||
return mapping_2d::GetXYIndexOfCellContainingPoint(
|
// Index values are row major and the top left has Eigen::Array2i::Zero()
|
||||||
x, y, centered_limits_.max().x(), centered_limits_.max().y(),
|
// and contains (centered_max_x, centered_max_y). We need to flip and
|
||||||
resolution_);
|
// 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'.
|
// Returns true of the ProbabilityGrid contains 'xy_index'.
|
||||||
|
@ -129,18 +132,22 @@ class MapLimits {
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
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
|
// Sets the cell size to the specified resolution in meters and the limits of
|
||||||
// the grid to the specified bounding box in meters.
|
// the grid to the specified bounding box in meters.
|
||||||
void SetLimits(double resolution, const Eigen::AlignedBox2d& limits) {
|
void SetLimits(double resolution, const Eigen::AlignedBox2d& limits) {
|
||||||
CHECK(!limits.isEmpty());
|
CHECK(!limits.isEmpty());
|
||||||
const int num_x_cells =
|
resolution_ = resolution;
|
||||||
common::RoundToInt((mapping_2d::Center(limits.max().y(), resolution) -
|
const int num_x_cells = common::RoundToInt((Center(limits.max().y()) -
|
||||||
mapping_2d::Center(limits.min().y(), resolution)) /
|
Center(limits.min().y())) /
|
||||||
resolution) +
|
resolution) +
|
||||||
1;
|
1;
|
||||||
const int num_y_cells =
|
const int num_y_cells = common::RoundToInt((Center(limits.max().x()) -
|
||||||
common::RoundToInt((mapping_2d::Center(limits.max().x(), resolution) -
|
Center(limits.min().x())) /
|
||||||
mapping_2d::Center(limits.min().x(), resolution)) /
|
|
||||||
resolution) +
|
resolution) +
|
||||||
1;
|
1;
|
||||||
SetLimits(resolution, limits.max().x(), limits.max().y(),
|
SetLimits(resolution, limits.max().x(), limits.max().y(),
|
||||||
|
@ -163,8 +170,8 @@ class MapLimits {
|
||||||
|
|
||||||
resolution_ = resolution;
|
resolution_ = resolution;
|
||||||
cell_limits_ = limits;
|
cell_limits_ = limits;
|
||||||
centered_limits_.max().x() = Center(max_x, resolution);
|
centered_limits_.max().x() = Center(max_x);
|
||||||
centered_limits_.max().y() = Center(max_y, resolution);
|
centered_limits_.max().y() = Center(max_y);
|
||||||
centered_limits_.min().x() = centered_limits_.max().x() -
|
centered_limits_.min().x() = centered_limits_.max().x() -
|
||||||
resolution_ * (cell_limits_.num_y_cells - 1);
|
resolution_ * (cell_limits_.num_y_cells - 1);
|
||||||
centered_limits_.min().y() = centered_limits_.max().y() -
|
centered_limits_.min().y() = centered_limits_.max().y() -
|
||||||
|
|
|
@ -111,15 +111,6 @@ class ProbabilityGrid {
|
||||||
mapping::kUnknownProbabilityValue;
|
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
|
// Fills in 'offset' and 'limits' to define a subregion of that contains all
|
||||||
// known cells.
|
// known cells.
|
||||||
void ComputeCroppedLimits(Eigen::Array2i* const offset,
|
void ComputeCroppedLimits(Eigen::Array2i* const offset,
|
||||||
|
@ -168,8 +159,7 @@ class ProbabilityGrid {
|
||||||
// Returns the index of the cell at 'xy_index'.
|
// Returns the index of the cell at 'xy_index'.
|
||||||
int GetIndexOfCell(const Eigen::Array2i& xy_index) const {
|
int GetIndexOfCell(const Eigen::Array2i& xy_index) const {
|
||||||
CHECK(limits_.Contains(xy_index)) << xy_index;
|
CHECK(limits_.Contains(xy_index)) << xy_index;
|
||||||
return mapping_2d::GetIndexOfCell(xy_index,
|
return limits_.cell_limits().num_x_cells * xy_index.y() + xy_index.x();
|
||||||
limits_.cell_limits().num_x_cells);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void UpdateBounds(const Eigen::Array2i& xy_index) {
|
void UpdateBounds(const Eigen::Array2i& xy_index) {
|
||||||
|
|
|
@ -144,26 +144,6 @@ TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) {
|
||||||
.all());
|
.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) {
|
TEST(ProbabilityGridTest, CorrectCropping) {
|
||||||
// Create a probability grid with random values.
|
// Create a probability grid with random values.
|
||||||
std::mt19937 rng(42);
|
std::mt19937 rng(42);
|
||||||
|
|
|
@ -150,20 +150,25 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
|
||||||
void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
|
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&)>& hit_visitor,
|
||||||
const std::function<void(const Eigen::Array2i&)>& miss_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 =
|
const Eigen::Vector2d superscaled_centered_max =
|
||||||
limits.edge_limits().max() - resolution / 2. * Eigen::Vector2d::Ones();
|
limits.edge_limits().max() -
|
||||||
const Eigen::Array2i begin = GetXYIndexOfCellContainingPoint(
|
superscaled_resolution / 2. * Eigen::Vector2d::Ones();
|
||||||
laser_fan.origin.x(), laser_fan.origin.y(), superscaled_centered_max.x(),
|
const MapLimits superscaled_limits(
|
||||||
superscaled_centered_max.y(), resolution);
|
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.
|
// Compute and add the end points.
|
||||||
std::vector<Eigen::Array2i> ends;
|
std::vector<Eigen::Array2i> ends;
|
||||||
ends.reserve(laser_fan.point_cloud.size());
|
ends.reserve(laser_fan.point_cloud.size());
|
||||||
for (const Eigen::Vector2f& laser_return : laser_fan.point_cloud) {
|
for (const Eigen::Vector2f& laser_return : laser_fan.point_cloud) {
|
||||||
ends.push_back(GetXYIndexOfCellContainingPoint(
|
ends.push_back(superscaled_limits.GetXYIndexOfCellContainingPoint(
|
||||||
laser_return.x(), laser_return.y(), superscaled_centered_max.x(),
|
laser_return.x(), laser_return.y()));
|
||||||
superscaled_centered_max.y(), resolution));
|
|
||||||
hit_visitor(ends.back() / kSubpixelScale);
|
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.
|
// Finally, compute and add empty rays based on missing echos in the scan.
|
||||||
for (const Eigen::Vector2f& missing_echo :
|
for (const Eigen::Vector2f& missing_echo :
|
||||||
laser_fan.missing_echo_point_cloud) {
|
laser_fan.missing_echo_point_cloud) {
|
||||||
CastRay(begin, GetXYIndexOfCellContainingPoint(
|
CastRay(begin, superscaled_limits.GetXYIndexOfCellContainingPoint(
|
||||||
missing_echo.x(), missing_echo.y(),
|
missing_echo.x(), missing_echo.y()),
|
||||||
superscaled_centered_max.x(),
|
|
||||||
superscaled_centered_max.y(), resolution),
|
|
||||||
miss_visitor);
|
miss_visitor);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -117,31 +117,6 @@ class XYIndexRangeIterator
|
||||||
Eigen::Array2i xy_index_;
|
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 mapping_2d
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue