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. // Returns the limits of the grid in number of cells.
const CellLimits& cell_limits() const { return cell_limits_; } const CellLimits& cell_limits() const { return cell_limits_; }
// Returns the index of the cell containing the point ('x', 'y') which may be // Returns the index of the cell containing the 'point' which may be outside
// outside the map, i.e., negative or too large indices that will return // the map, i.e., negative or too large indices that will return false for
// false for Contains(). // Contains().
Eigen::Array2i GetXYIndexOfCellContainingPoint(const double x, Eigen::Array2i GetCellIndex(const Eigen::Vector2f& point) const {
const double y) const {
// Index values are row major and the top left has Eigen::Array2i::Zero() // 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 // and contains (centered_max_x, centered_max_y). We need to flip and
// rotate. // rotate.
return Eigen::Array2i( return Eigen::Array2i(
common::RoundToInt((max_.y() - y) / resolution_ - 0.5), common::RoundToInt((max_.y() - point.y()) / resolution_ - 0.5),
common::RoundToInt((max_.x() - x) / resolution_ - 0.5)); common::RoundToInt((max_.x() - point.x()) / resolution_ - 0.5));
} }
// Returns true of the ProbabilityGrid contains 'xy_index'. // Returns true if the ProbabilityGrid contains 'cell_index'.
bool Contains(const Eigen::Array2i& xy_index) const { bool Contains(const Eigen::Array2i& cell_index) const {
return (Eigen::Array2i(0, 0) <= xy_index).all() && return (Eigen::Array2i(0, 0) <= cell_index).all() &&
(xy_index < (cell_index <
Eigen::Array2i(cell_limits_.num_x_cells, cell_limits_.num_y_cells)) Eigen::Array2i(cell_limits_.num_x_cells, cell_limits_.num_y_cells))
.all(); .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'. // Sets the probability of the cell at 'cell_index' to the given
// Only allowed if the cell was unknown before. // 'probability'. Only allowed if the cell was unknown before.
void SetProbability(const Eigen::Array2i& xy_index, const float probability) { void SetProbability(const Eigen::Array2i& cell_index,
uint16& cell = cells_[GetIndexOfCell(xy_index)]; const float probability) {
uint16& cell = cells_[ToFlatIndex(cell_index)];
CHECK_EQ(cell, mapping::kUnknownProbabilityValue); CHECK_EQ(cell, mapping::kUnknownProbabilityValue);
cell = mapping::ProbabilityToValue(probability); 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() // 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 // been updated. Multiple updates of the same cell will be ignored until
// StartUpdate() is called. Returns true if the cell was updated. // StartUpdate() is called. Returns true if the cell was updated.
// //
// If this is the first call to ApplyOdds() for the specified cell, its value // If this is the first call to ApplyOdds() for the specified cell, its value
// will be set to probability corresponding to 'odds'. // 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) { const std::vector<uint16>& table) {
DCHECK_EQ(table.size(), mapping::kUpdateMarker); DCHECK_EQ(table.size(), mapping::kUpdateMarker);
const int cell_index = GetIndexOfCell(xy_index); const int flat_index = ToFlatIndex(cell_index);
uint16& cell = cells_[cell_index]; uint16& cell = cells_[flat_index];
if (cell >= mapping::kUpdateMarker) { if (cell >= mapping::kUpdateMarker) {
return false; return false;
} }
update_indices_.push_back(cell_index); update_indices_.push_back(flat_index);
cell = table[cell]; cell = table[cell];
DCHECK_GE(cell, mapping::kUpdateMarker); DCHECK_GE(cell, mapping::kUpdateMarker);
known_cells_box_.extend(xy_index.matrix()); known_cells_box_.extend(cell_index.matrix());
return true; return true;
} }
// Returns the probability of the cell with 'xy_index'. // Returns the probability of the cell with 'cell_index'.
float GetProbability(const Eigen::Array2i& xy_index) const { float GetProbability(const Eigen::Array2i& cell_index) const {
if (limits_.Contains(xy_index)) { if (limits_.Contains(cell_index)) {
return mapping::ValueToProbability(cells_[GetIndexOfCell(xy_index)]); return mapping::ValueToProbability(cells_[ToFlatIndex(cell_index)]);
} }
return mapping::kMinProbability; 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. // Returns true if the probability at the specified index is known.
bool IsKnown(const Eigen::Array2i& xy_index) const { bool IsKnown(const Eigen::Array2i& cell_index) const {
return limits_.Contains(xy_index) && cells_[GetIndexOfCell(xy_index)] != return limits_.Contains(cell_index) &&
mapping::kUnknownProbabilityValue; cells_[ToFlatIndex(cell_index)] != mapping::kUnknownProbabilityValue;
} }
// 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
@ -138,12 +134,12 @@ class ProbabilityGrid {
} }
} }
// Grows the map as necessary to include 'x' and 'y'. This changes the meaning // Grows the map as necessary to include 'point'. This changes the meaning of
// of these coordinates going forward. This method must be called immediately // these coordinates going forward. This method must be called immediately
// after 'StartUpdate', before any calls to 'ApplyLookupTable'. // 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()); 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 x_offset = limits_.cell_limits().num_x_cells / 2;
const int y_offset = limits_.cell_limits().num_y_cells / 2; const int y_offset = limits_.cell_limits().num_y_cells / 2;
const MapLimits new_limits( const MapLimits new_limits(
@ -193,10 +189,10 @@ class ProbabilityGrid {
} }
private: private:
// Returns the index of the cell at 'xy_index'. // Converts a 'cell_index' into an index into 'cells_'.
int GetIndexOfCell(const Eigen::Array2i& xy_index) const { int ToFlatIndex(const Eigen::Array2i& cell_index) const {
CHECK(limits_.Contains(xy_index)) << xy_index; CHECK(limits_.Contains(cell_index)) << cell_index;
return limits_.cell_limits().num_x_cells * xy_index.y() + xy_index.x(); return limits_.cell_limits().num_x_cells * cell_index.y() + cell_index.x();
} }
MapLimits limits_; MapLimits limits_;

37
cartographer/mapping_2d/probability_grid_test.cc
View File

@ -125,20 +125,21 @@ TEST(ProbabilityGridTest, GetProbability) {
probability_grid.StartUpdate(); probability_grid.StartUpdate();
probability_grid.SetProbability( probability_grid.SetProbability(
limits.GetXYIndexOfCellContainingPoint(-0.5, 0.5), limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f)),
mapping::kMaxProbability); 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); mapping::kMaxProbability, 1e-6);
for (const Eigen::Array2i& xy_index : for (const Eigen::Array2i& xy_index :
{limits.GetXYIndexOfCellContainingPoint(-0.5, 1.5), {limits.GetCellIndex(Eigen::Vector2f(-0.5f, 1.5f)),
limits.GetXYIndexOfCellContainingPoint(0.5, 0.5), limits.GetCellIndex(Eigen::Vector2f(0.5f, 0.5f)),
limits.GetXYIndexOfCellContainingPoint(0.5, 1.5)}) { limits.GetCellIndex(Eigen::Vector2f(0.5f, 1.5f))}) {
EXPECT_TRUE(limits.Contains(xy_index)); EXPECT_TRUE(limits.Contains(xy_index));
EXPECT_FALSE(probability_grid.IsKnown(xy_index)); EXPECT_FALSE(probability_grid.IsKnown(xy_index));
} }
} }
TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) { TEST(ProbabilityGridTest, GetCellIndex) {
ProbabilityGrid probability_grid( ProbabilityGrid probability_grid(
MapLimits(2., Eigen::Vector2d(8., 14.), CellLimits(14, 8))); 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(14, cell_limits.num_x_cells);
ASSERT_EQ(8, cell_limits.num_y_cells); ASSERT_EQ(8, cell_limits.num_y_cells);
EXPECT_TRUE( 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()); .all());
EXPECT_TRUE( EXPECT_TRUE(
(Eigen::Array2i(13, 0) == limits.GetXYIndexOfCellContainingPoint(7, -13)) (Eigen::Array2i(0, 7) == limits.GetCellIndex(Eigen::Vector2f(-7.f, 13.f)))
.all()); .all());
EXPECT_TRUE( EXPECT_TRUE((Eigen::Array2i(13, 7) ==
(Eigen::Array2i(0, 7) == limits.GetXYIndexOfCellContainingPoint(-7, 13)) limits.GetCellIndex(Eigen::Vector2f(-7.f, -13.f)))
.all());
EXPECT_TRUE(
(Eigen::Array2i(13, 7) == limits.GetXYIndexOfCellContainingPoint(-7, -13))
.all()); .all());
// Check around the origin. // Check around the origin.
EXPECT_TRUE( 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()); .all());
EXPECT_TRUE( 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()); .all());
EXPECT_TRUE((Eigen::Array2i(7, 3) == EXPECT_TRUE((Eigen::Array2i(7, 3) ==
limits.GetXYIndexOfCellContainingPoint(0.5, -0.5)) limits.GetCellIndex(Eigen::Vector2f(0.5f, -0.5f)))
.all()); .all());
EXPECT_TRUE((Eigen::Array2i(6, 4) == EXPECT_TRUE((Eigen::Array2i(6, 4) ==
limits.GetXYIndexOfCellContainingPoint(-0.5, 0.5)) limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f)))
.all()); .all());
EXPECT_TRUE((Eigen::Array2i(7, 4) == EXPECT_TRUE((Eigen::Array2i(7, 4) ==
limits.GetXYIndexOfCellContainingPoint(-0.5, -0.5)) limits.GetCellIndex(Eigen::Vector2f(-0.5f, -0.5f)))
.all()); .all());
} }

50
cartographer/mapping_2d/range_data_inserter_test.cc
View File

@ -45,12 +45,12 @@ class RangeDataInserterTest : public ::testing::Test {
void InsertPointCloud() { void InsertPointCloud() {
sensor::RangeData range_data; sensor::RangeData range_data;
range_data.returns.emplace_back(-3.5, 0.5, 0.f); range_data.returns.emplace_back(-3.5f, 0.5f, 0.f);
range_data.returns.emplace_back(-2.5, 1.5, 0.f); range_data.returns.emplace_back(-2.5f, 1.5f, 0.f);
range_data.returns.emplace_back(-1.5, 2.5, 0.f); range_data.returns.emplace_back(-1.5f, 2.5f, 0.f);
range_data.returns.emplace_back(-0.5, 3.5, 0.f); range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
range_data.origin.x() = -0.5; range_data.origin.x() = -0.5f;
range_data.origin.y() = 0.5; range_data.origin.y() = 0.5f;
probability_grid_.StartUpdate(); probability_grid_.StartUpdate();
range_data_inserter_->Insert(range_data, &probability_grid_); range_data_inserter_->Insert(range_data, &probability_grid_);
} }
@ -81,19 +81,19 @@ TEST_F(RangeDataInserterTest, InsertPointCloud) {
State::HIT}}; State::HIT}};
for (int row = 0; row != 5; ++row) { for (int row = 0; row != 5; ++row) {
for (int column = 0; column != 5; ++column) { for (int column = 0; column != 5; ++column) {
Eigen::Array2i xy_index(row, column); Eigen::Array2i cell_index(row, column);
EXPECT_TRUE(probability_grid_.limits().Contains(xy_index)); EXPECT_TRUE(probability_grid_.limits().Contains(cell_index));
switch (expected_states[column][row]) { switch (expected_states[column][row]) {
case State::UNKNOWN: case State::UNKNOWN:
EXPECT_FALSE(probability_grid_.IsKnown(xy_index)); EXPECT_FALSE(probability_grid_.IsKnown(cell_index));
break; break;
case State::MISS: case State::MISS:
EXPECT_NEAR(options_.miss_probability(), EXPECT_NEAR(options_.miss_probability(),
probability_grid_.GetProbability(xy_index), 1e-4); probability_grid_.GetProbability(cell_index), 1e-4);
break; break;
case State::HIT: case State::HIT:
EXPECT_NEAR(options_.hit_probability(), EXPECT_NEAR(options_.hit_probability(),
probability_grid_.GetProbability(xy_index), 1e-4); probability_grid_.GetProbability(cell_index), 1e-4);
break; break;
} }
} }
@ -102,18 +102,30 @@ TEST_F(RangeDataInserterTest, InsertPointCloud) {
TEST_F(RangeDataInserterTest, ProbabilityProgression) { TEST_F(RangeDataInserterTest, ProbabilityProgression) {
InsertPointCloud(); InsertPointCloud();
EXPECT_NEAR(options_.hit_probability(), EXPECT_NEAR(
probability_grid_.GetProbability(-3.5, 0.5), 1e-4); options_.hit_probability(),
EXPECT_NEAR(options_.miss_probability(), probability_grid_.GetProbability(probability_grid_.limits().GetCellIndex(
probability_grid_.GetProbability(-2.5, 0.5), 1e-4); 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) { for (int i = 0; i < 1000; ++i) {
InsertPointCloud(); InsertPointCloud();
} }
EXPECT_NEAR(mapping::kMaxProbability, EXPECT_NEAR(
probability_grid_.GetProbability(-3.5, 0.5), 1e-3); mapping::kMaxProbability,
EXPECT_NEAR(mapping::kMinProbability, probability_grid_.GetProbability(probability_grid_.limits().GetCellIndex(
probability_grid_.GetProbability(-2.5, 0.5), 1e-3); 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 } // 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) { for (const Eigen::Vector3f& miss : range_data.misses) {
bounding_box.extend(miss.head<2>()); bounding_box.extend(miss.head<2>());
} }
probability_grid->GrowLimits(bounding_box.min().x() - kPadding, probability_grid->GrowLimits(bounding_box.min() -
bounding_box.min().y() - kPadding); kPadding * Eigen::Vector2f::Ones());
probability_grid->GrowLimits(bounding_box.max().x() + kPadding, probability_grid->GrowLimits(bounding_box.max() +
bounding_box.max().y() + kPadding); kPadding * Eigen::Vector2f::Ones());
} }
} // namespace } // namespace
@ -178,14 +178,12 @@ void CastRays(const sensor::RangeData& range_data,
CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale, CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
limits.cell_limits().num_y_cells * kSubpixelScale)); limits.cell_limits().num_y_cells * kSubpixelScale));
const Eigen::Array2i begin = const Eigen::Array2i begin =
superscaled_limits.GetXYIndexOfCellContainingPoint(range_data.origin.x(), superscaled_limits.GetCellIndex(range_data.origin.head<2>());
range_data.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(range_data.returns.size()); ends.reserve(range_data.returns.size());
for (const Eigen::Vector3f& hit : range_data.returns) { for (const Eigen::Vector3f& hit : range_data.returns) {
ends.push_back( ends.push_back(superscaled_limits.GetCellIndex(hit.head<2>()));
superscaled_limits.GetXYIndexOfCellContainingPoint(hit.x(), hit.y()));
probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table); 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. // Finally, compute and add empty rays based on misses in the scan.
for (const Eigen::Vector3f& missing_echo : range_data.misses) { for (const Eigen::Vector3f& missing_echo : range_data.misses) {
CastRay(begin, CastRay(begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()),
superscaled_limits.GetXYIndexOfCellContainingPoint(
missing_echo.x(), missing_echo.y()),
miss_table, probability_grid); 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_( : probability_grid_(
MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20))) { MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20))) {
probability_grid_.SetProbability( probability_grid_.SetProbability(
probability_grid_.limits().GetXYIndexOfCellContainingPoint(-3.5, 2.5), probability_grid_.limits().GetCellIndex(Eigen::Vector2f(-3.5f, 2.5f)),
mapping::kMaxProbability); mapping::kMaxProbability);
point_cloud_.emplace_back(-3.f, 2.f, 0.f); 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 = const Eigen::Vector2f translated_point =
Eigen::Affine2f(initial_translation) * point.head<2>(); Eigen::Affine2f(initial_translation) * point.head<2>();
discrete_scans.back().push_back( discrete_scans.back().push_back(
map_limits.GetXYIndexOfCellContainingPoint(translated_point.x(), map_limits.GetCellIndex(translated_point));
translated_point.y()));
} }
} }
return discrete_scans; return discrete_scans;

6
cartographer/sensor/range_data.cc
View File

@ -48,14 +48,16 @@ proto::CompressedRangeData ToProto(
CompressedRangeData FromProto(const proto::CompressedRangeData& proto) { CompressedRangeData FromProto(const proto::CompressedRangeData& proto) {
return CompressedRangeData{ return CompressedRangeData{
transform::ToEigen(proto.origin()), CompressedPointCloud(proto.returns()), transform::ToEigen(proto.origin()),
CompressedPointCloud(proto.returns()),
CompressedPointCloud(proto.misses()), CompressedPointCloud(proto.misses()),
}; };
} }
CompressedRangeData Compress(const RangeData& range_data) { CompressedRangeData Compress(const RangeData& range_data) {
return CompressedRangeData{ return CompressedRangeData{
range_data.origin, CompressedPointCloud(range_data.returns), range_data.origin,
CompressedPointCloud(range_data.returns),
CompressedPointCloud(range_data.misses), CompressedPointCloud(range_data.misses),
}; };
} }