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

Improve handling of grid updates. (#400) 

Changes the grids to only contain update markers during updates.
Serialized grids will never contain them.
master
Wolfgang Hess 2017-07-10 16:14:50 +02:00 committed by Damon Kohler
parent b0b4f30007
commit 0da3fad9b0
12 changed files with 33 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
cartographer/mapping_2d/probability_grid.h
View File

@ -46,10 +46,7 @@ class ProbabilityGrid {
mapping::kUnknownProbabilityValue) {}
explicit ProbabilityGrid(const proto::ProbabilityGrid& proto)
: limits_(proto.limits()),
cells_(),
update_indices_(proto.update_indices().begin(),
proto.update_indices().end()) {
: limits_(proto.limits()), cells_() {
if (proto.has_min_x()) {
known_cells_box_ =
Eigen::AlignedBox2i(Eigen::Vector2i(proto.min_x(), proto.min_y()),
@ -65,8 +62,8 @@ class ProbabilityGrid {
// Returns the limits of this ProbabilityGrid.
const MapLimits& limits() const { return limits_; }
// Starts the next update sequence.
void StartUpdate() {
// Finishes the update sequence.
void FinishUpdate() {
while (!update_indices_.empty()) {
DCHECK_GE(cells_[update_indices_.back()], mapping::kUpdateMarker);
cells_[update_indices_.back()] -= mapping::kUpdateMarker;
@ -87,7 +84,7 @@ class ProbabilityGrid {
// Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
// 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.
// FinishUpdate() 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'.
@ -136,7 +133,7 @@ class ProbabilityGrid {
// 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'.
// after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
void GrowLimits(const Eigen::Vector2f& point) {
CHECK(update_indices_.empty());
while (!limits_.Contains(limits_.GetCellIndex(point))) {
@ -175,10 +172,9 @@ class ProbabilityGrid {
for (const auto cell : cells_) {
result.mutable_cells()->Add(cell);
}
result.mutable_update_indices()->Reserve(update_indices_.size());
for (const auto update : update_indices_) {
result.mutable_update_indices()->Add(update);
}
CHECK(update_indices_.empty()) << "Serialiazing a probability grid during "
"an update is not supported. Finish the "
"update first.";
if (!known_cells_box_.isEmpty()) {
result.set_max_x(known_cells_box_.max().x());
result.set_max_y(known_cells_box_.max().y());

23
cartographer/mapping_2d/probability_grid_test.cc
View File

@ -32,9 +32,6 @@ TEST(ProbabilityGridTest, ProtoConstructor) {
for (int i = 6; i < 12; ++i) {
proto.mutable_cells()->Add(static_cast<uint16>(i));
}
for (int i = 13; i < 18; ++i) {
proto.mutable_update_indices()->Add(i);
}
proto.set_max_x(19);
proto.set_max_y(20);
proto.set_min_x(21);
@ -43,8 +40,8 @@ TEST(ProbabilityGridTest, ProtoConstructor) {
ProbabilityGrid grid(proto);
EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString());
// TODO(macmason): Figure out how to test the contents of cells_,
// update_indices_, and {min, max}_{x, y}_ gracefully.
// TODO(macmason): Figure out how to test the contents of cells_ and
// {min, max}_{x, y}_ gracefully.
}
TEST(ProbabilityGridTest, ToProto) {
@ -55,8 +52,8 @@ TEST(ProbabilityGridTest, ToProto) {
EXPECT_EQ(ToProto(probability_grid.limits()).DebugString(),
proto.limits().DebugString());
// TODO(macmason): Figure out how to test the contents of cells_,
// update_indices_, and {min, max}_{x, y}_ gracefully.
// TODO(macmason): Figure out how to test the contents of cells_ and
// {min, max}_{x, y}_ gracefully.
}
TEST(ProbabilityGridTest, ApplyOdds) {
@ -75,36 +72,34 @@ TEST(ProbabilityGridTest, ApplyOdds) {
probability_grid.SetProbability(Eigen::Array2i(1, 0), 0.5);
probability_grid.StartUpdate();
probability_grid.ApplyLookupTable(
Eigen::Array2i(1, 0),
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
probability_grid.FinishUpdate();
EXPECT_GT(probability_grid.GetProbability(Eigen::Array2i(1, 0)), 0.5);
probability_grid.StartUpdate();
probability_grid.SetProbability(Eigen::Array2i(0, 1), 0.5);
probability_grid.StartUpdate();
probability_grid.ApplyLookupTable(
Eigen::Array2i(0, 1),
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1)));
probability_grid.FinishUpdate();
EXPECT_LT(probability_grid.GetProbability(Eigen::Array2i(0, 1)), 0.5);
// Tests adding odds to an unknown cell.
probability_grid.StartUpdate();
probability_grid.ApplyLookupTable(
Eigen::Array2i(1, 1),
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42)));
EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42,
1e-4);
// Tests that further updates are ignored if StartUpdate() isn't called.
// Tests that further updates are ignored if FinishUpdate() isn't called.
probability_grid.ApplyLookupTable(
Eigen::Array2i(1, 1),
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42,
1e-4);
probability_grid.StartUpdate();
probability_grid.FinishUpdate();
probability_grid.ApplyLookupTable(
Eigen::Array2i(1, 1),
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
@ -123,7 +118,6 @@ TEST(ProbabilityGridTest, GetProbability) {
ASSERT_EQ(2, cell_limits.num_x_cells);
ASSERT_EQ(2, cell_limits.num_y_cells);
probability_grid.StartUpdate();
probability_grid.SetProbability(
limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f)),
mapping::kMaxProbability);
@ -185,7 +179,6 @@ TEST(ProbabilityGridTest, CorrectCropping) {
mapping::kMinProbability, mapping::kMaxProbability);
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)));
probability_grid.StartUpdate();
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(
Eigen::Array2i(100, 100), Eigen::Array2i(299, 299))) {
probability_grid.SetProbability(xy_index, value_distribution(rng));

1
cartographer/mapping_2d/proto/probability_grid.proto
View File

@ -23,7 +23,6 @@ message ProbabilityGrid {
// These values are actually int16s, but protos don't have a native int16
// type.
repeated int32 cells = 2;
repeated int32 update_indices = 3;
optional int32 max_x = 4;
optional int32 max_y = 5;
optional int32 min_x = 6;

6
cartographer/mapping_2d/range_data_inserter.cc
View File

@ -53,11 +53,11 @@ RangeDataInserter::RangeDataInserter(
void RangeDataInserter::Insert(const sensor::RangeData& range_data,
ProbabilityGrid* const probability_grid) const {
// By not starting a new update after hits are inserted, we give hits priority
// By not finishing the update after hits are inserted, we give hits priority
// (i.e. no hits will be ignored because of a miss in the same cell).
CHECK_NOTNULL(probability_grid)->StartUpdate();
CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),
probability_grid);
CHECK_NOTNULL(probability_grid));
probability_grid->FinishUpdate();
}
} // namespace mapping_2d

2
cartographer/mapping_2d/range_data_inserter_test.cc
View File

@ -51,8 +51,8 @@ class RangeDataInserterTest : public ::testing::Test {
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_);
probability_grid_.FinishUpdate();
}
ProbabilityGrid probability_grid_;

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

@ -41,7 +41,6 @@ TEST(PrecomputationGridTest, CorrectValues) {
std::uniform_int_distribution<int> distribution(0, 255);
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250)));
probability_grid.StartUpdate();
for (const Eigen::Array2i& xy_index :
XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) {
probability_grid.SetProbability(
@ -76,7 +75,6 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) {
std::uniform_int_distribution<int> distribution(0, 255);
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4)));
probability_grid.StartUpdate();
for (const Eigen::Array2i& xy_index :
XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
probability_grid.SetProbability(
@ -151,7 +149,6 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
probability_grid.StartUpdate();
range_data_inserter.Insert(
sensor::RangeData{
Eigen::Vector3f(expected_pose.translation().x(),
@ -160,6 +157,7 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
point_cloud, transform::Embed3D(expected_pose.cast<float>())),
{}},
&probability_grid);
probability_grid.FinishUpdate();
FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
options);
@ -204,7 +202,6 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
probability_grid.StartUpdate();
range_data_inserter.Insert(
sensor::RangeData{
transform::Embed3D(expected_pose * perturbation).translation(),
@ -212,6 +209,7 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
transform::Embed3D(expected_pose)),
{}},
&probability_grid);
probability_grid.FinishUpdate();
FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
options);

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

@ -55,10 +55,10 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
point_cloud_.emplace_back(-0.125f, 0.125f, 0.f);
point_cloud_.emplace_back(-0.125f, 0.075f, 0.f);
point_cloud_.emplace_back(-0.125f, 0.025f, 0.f);
probability_grid_.StartUpdate();
range_data_inserter_->Insert(
sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}},
&probability_grid_);
probability_grid_.FinishUpdate();
{
auto parameter_dictionary = common::MakeDictionary(
"return {"

1
cartographer/mapping_2d/submaps.cc
View File

@ -40,7 +40,6 @@ ProbabilityGrid ComputeCroppedProbabilityGrid(
probability_grid.limits().max() -
resolution * Eigen::Vector2d(offset.y(), offset.x());
ProbabilityGrid cropped_grid(MapLimits(resolution, max, limits));
cropped_grid.StartUpdate();
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
if (probability_grid.IsKnown(xy_index + offset)) {
cropped_grid.SetProbability(

6
cartographer/mapping_3d/hybrid_grid.h
View File

@ -485,8 +485,8 @@ class HybridGrid : public HybridGridBase<uint16> {
*mutable_value(index) = mapping::ProbabilityToValue(probability);
}
// Starts the next update sequence.
void StartUpdate() {
// Finishes the update sequence.
void FinishUpdate() {
while (!update_indices_.empty()) {
DCHECK_GE(*update_indices_.back(), mapping::kUpdateMarker);
*update_indices_.back() -= mapping::kUpdateMarker;
@ -497,7 +497,7 @@ class HybridGrid : public HybridGridBase<uint16> {
// Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
// to the probability of the cell at '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.
// FinishUpdate() 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'.

9
cartographer/mapping_3d/hybrid_grid_test.cc
View File

@ -40,33 +40,32 @@ TEST(HybridGridTest, ApplyOdds) {
hybrid_grid.SetProbability(Eigen::Array3i(1, 0, 1), 0.5f);
hybrid_grid.StartUpdate();
hybrid_grid.ApplyLookupTable(
Eigen::Array3i(1, 0, 1),
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f)));
hybrid_grid.FinishUpdate();
EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 0, 1)), 0.5f);
hybrid_grid.SetProbability(Eigen::Array3i(0, 1, 0), 0.5f);
hybrid_grid.StartUpdate();
hybrid_grid.ApplyLookupTable(
Eigen::Array3i(0, 1, 0),
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1f)));
hybrid_grid.FinishUpdate();
EXPECT_LT(hybrid_grid.GetProbability(Eigen::Array3i(0, 1, 0)), 0.5f);
// Tests adding odds to an unknown cell.
hybrid_grid.StartUpdate();
hybrid_grid.ApplyLookupTable(
Eigen::Array3i(1, 1, 1),
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42f)));
EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4);
// Tests that further updates are ignored if StartUpdate() isn't called.
// Tests that further updates are ignored if FinishUpdate() isn't called.
hybrid_grid.ApplyLookupTable(
Eigen::Array3i(1, 1, 1),
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f)));
EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4);
hybrid_grid.StartUpdate();
hybrid_grid.FinishUpdate();
hybrid_grid.ApplyLookupTable(
Eigen::Array3i(1, 1, 1),
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f)));

3
cartographer/mapping_3d/range_data_inserter.cc
View File

@ -78,7 +78,7 @@ RangeDataInserter::RangeDataInserter(
void RangeDataInserter::Insert(const sensor::RangeData& range_data,
HybridGrid* hybrid_grid) const {
CHECK_NOTNULL(hybrid_grid)->StartUpdate();
CHECK_NOTNULL(hybrid_grid);
for (const Eigen::Vector3f& hit : range_data.returns) {
const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit);
@ -89,6 +89,7 @@ void RangeDataInserter::Insert(const sensor::RangeData& range_data,
// (i.e. no hits will be ignored because of a miss in the same cell).
InsertMissesIntoGrid(miss_table_, range_data.origin, range_data.returns,
hybrid_grid, options_.num_free_space_voxels());
hybrid_grid->FinishUpdate();
}
} // namespace mapping_3d

2
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc
View File

@ -88,13 +88,13 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
HybridGrid hybrid_grid(0.05f);
hybrid_grid.StartUpdate();
range_data_inserter.Insert(
sensor::RangeData{
expected_pose.translation(),
sensor::TransformPointCloud(point_cloud, expected_pose),
{}},
&hybrid_grid);
hybrid_grid.FinishUpdate();
FastCorrelativeScanMatcher fast_correlative_scan_matcher(hybrid_grid, {},
options);