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

Clean up of XYIndexRangeIterator and MapLimits. (#10)

master
Wolfgang Hess 2016-08-24 18:01:14 +02:00 committed by GitHub
parent b50f1a6e96
commit 4c6a2fcb28
5 changed files with 22 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
cartographer/mapping_2d/map_limits.h
View File

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

3
cartographer/mapping_2d/probability_grid_test.cc
View File

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

3
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc
View File

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

31
cartographer/mapping_2d/xy_index.h
View File

@ -43,15 +43,12 @@ struct CellLimits {
class XYIndexRangeIterator
: public std::iterator<std::input_iterator_tag, Eigen::Array2i> {
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_;

6
cartographer/mapping_2d/xy_index_test.cc
View File

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