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

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().
master
Wolfgang Hess 2017-07-10 15:01:13 +02:00 committed by Damon Kohler
parent 39cb8401a5
commit b0b4f30007
8 changed files with 101 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
cartographer/mapping_2d/map_limits.h
View File

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

58
cartographer/mapping_2d/probability_grid.h
View File

@ -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<uint16>& 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_;

39
cartographer/mapping_2d/probability_grid_test.cc
View File

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

50
cartographer/mapping_2d/range_data_inserter_test.cc
View File

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

18
cartographer/mapping_2d/ray_casting.cc
View File

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

2
cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc
View File

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

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

@ -120,8 +120,7 @@ std::vector<DiscreteScan> 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;

6
cartographer/sensor/range_data.cc
View File

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