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

Use intensities in `Submap3D` and `RangeDataInserter3D`. (#1765) 

`Submap3D` now also stores a pointer to `IntensityHybridGrid`.
Adapted `RangeDataInserter3D` to also insert intensities.

Signed-off-by: Wolfgang Hess <whess@lyft.com>
master
Wolfgang Hess 2020-10-22 09:24:47 +02:00 committed by GitHub
parent aeb8a10c01
commit a9ad813c26
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
10 changed files with 106 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer/io/hybrid_grid_points_processor.cc
View File

@ -40,7 +40,8 @@ HybridGridPointsProcessor::FromDictionary(
void HybridGridPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
range_data_inserter_.Insert(
{batch->origin, sensor::PointCloud(batch->points), {}}, &hybrid_grid_);
{batch->origin, sensor::PointCloud(batch->points), {}}, &hybrid_grid_,
/*intensity_hybrid_grid=*/nullptr);
next_->Process(std::move(batch));
}

28
cartographer/mapping/3d/range_data_inserter_3d.cc
View File

@ -51,6 +51,21 @@ void InsertMissesIntoGrid(const std::vector<uint16>& miss_table,
}
}
void InsertIntensitiesIntoGrid(const sensor::PointCloud& returns,
IntensityHybridGrid* intensity_hybrid_grid,
const float intensity_threshold) {
if (returns.intensities().size() > 0) {
for (size_t i = 0; i < returns.size(); ++i) {
if (returns.intensities()[i] > intensity_threshold) {
continue;
}
const Eigen::Array3i hit_cell =
intensity_hybrid_grid->GetCellIndex(returns[i].position);
intensity_hybrid_grid->AddIntensity(hit_cell, returns.intensities()[i]);
}
}
}
} // namespace
proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D(
@ -62,6 +77,8 @@ proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D(
parameter_dictionary->GetDouble("miss_probability"));
options.set_num_free_space_voxels(
parameter_dictionary->GetInt("num_free_space_voxels"));
options.set_intensity_threshold(
parameter_dictionary->GetDouble("intensity_threshold"));
CHECK_GT(options.hit_probability(), 0.5);
CHECK_LT(options.miss_probability(), 0.5);
return options;
@ -75,9 +92,10 @@ RangeDataInserter3D::RangeDataInserter3D(
miss_table_(
ComputeLookupTableToApplyOdds(Odds(options_.miss_probability()))) {}
void RangeDataInserter3D::Insert(const sensor::RangeData& range_data,
HybridGrid* hybrid_grid) const {
CHECK(hybrid_grid != nullptr);
void RangeDataInserter3D::Insert(
const sensor::RangeData& range_data, HybridGrid* hybrid_grid,
IntensityHybridGrid* intensity_hybrid_grid) const {
CHECK_NOTNULL(hybrid_grid);
for (const sensor::RangefinderPoint& hit : range_data.returns) {
const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position);
@ -88,6 +106,10 @@ void RangeDataInserter3D::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());
if (intensity_hybrid_grid != nullptr) {
InsertIntensitiesIntoGrid(range_data.returns, intensity_hybrid_grid,
options_.intensity_threshold());
}
hybrid_grid->FinishUpdate();
}

7
cartographer/mapping/3d/range_data_inserter_3d.h
View File

@ -36,9 +36,10 @@ class RangeDataInserter3D {
RangeDataInserter3D(const RangeDataInserter3D&) = delete;
RangeDataInserter3D& operator=(const RangeDataInserter3D&) = delete;
// Inserts 'range_data' into 'hybrid_grid'.
void Insert(const sensor::RangeData& range_data,
HybridGrid* hybrid_grid) const;
// Inserts 'range_data' into 'hybrid_grid' and optionally into
// 'intensity_hybrid_grid'.
void Insert(const sensor::RangeData& range_data, HybridGrid* hybrid_grid,
IntensityHybridGrid* intensity_hybrid_grid) const;
private:
const proto::RangeDataInserterOptions3D options_;

49
cartographer/mapping/3d/range_data_inserter_3d_test.cc
View File

@ -28,12 +28,13 @@ namespace {
class RangeDataInserter3DTest : public ::testing::Test {
protected:
RangeDataInserter3DTest() : hybrid_grid_(1.f) {
RangeDataInserter3DTest() : hybrid_grid_(1.f), intensity_hybrid_grid_(1.f) {
auto parameter_dictionary = common::MakeDictionary(
"return { "
"hit_probability = 0.7, "
"miss_probability = 0.4, "
"num_free_space_voxels = 1000, "
"intensity_threshold = 100, "
"}");
options_ = CreateRangeDataInserterOptions3D(parameter_dictionary.get());
range_data_inserter_.reset(new RangeDataInserter3D(options_));
@ -41,14 +42,28 @@ class RangeDataInserter3DTest : public ::testing::Test {
void InsertPointCloud() {
const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f);
std::vector<sensor::RangefinderPoint> returns = {
const std::vector<sensor::RangefinderPoint> returns = {
{Eigen::Vector3f{-3.f, -1.f, 4.f}},
{Eigen::Vector3f{-2.f, 0.f, 4.f}},
{Eigen::Vector3f{-1.f, 1.f, 4.f}},
{Eigen::Vector3f{0.f, 2.f, 4.f}}};
range_data_inserter_->Insert(
sensor::RangeData{origin, sensor::PointCloud(returns), {}},
&hybrid_grid_);
&hybrid_grid_,
/*intensity_hybrid_grid=*/nullptr);
}
void InsertPointCloudWithIntensities() {
const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f);
const std::vector<sensor::RangefinderPoint> returns = {
{Eigen::Vector3f{-3.f, -1.f, 4.f}},
{Eigen::Vector3f{-2.f, 0.f, 4.f}},
{Eigen::Vector3f{-1.f, 1.f, 4.f}},
{Eigen::Vector3f{0.f, 2.f, 4.f}}};
const std::vector<float> intensities{7.f, 8.f, 9.f, 10.f};
range_data_inserter_->Insert(
sensor::RangeData{origin, sensor::PointCloud(returns, intensities), {}},
&hybrid_grid_, &intensity_hybrid_grid_);
}
float GetProbability(float x, float y, float z) const {
@ -56,6 +71,11 @@ class RangeDataInserter3DTest : public ::testing::Test {
hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
}
float GetIntensity(float x, float y, float z) const {
return intensity_hybrid_grid_.GetIntensity(
intensity_hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
}
float IsKnown(float x, float y, float z) const {
return hybrid_grid_.IsKnown(
hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
@ -65,6 +85,7 @@ class RangeDataInserter3DTest : public ::testing::Test {
private:
HybridGrid hybrid_grid_;
IntensityHybridGrid intensity_hybrid_grid_;
std::unique_ptr<RangeDataInserter3D> range_data_inserter_;
proto::RangeDataInserterOptions3D options_;
};
@ -89,6 +110,28 @@ TEST_F(RangeDataInserter3DTest, InsertPointCloud) {
}
}
TEST_F(RangeDataInserter3DTest, InsertPointCloudWithIntensities) {
InsertPointCloudWithIntensities();
EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -4.f),
1e-4);
EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -3.f),
1e-4);
EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -2.f),
1e-4);
for (int x = -4; x <= 4; ++x) {
for (int y = -4; y <= 4; ++y) {
if (x < -3 || x > 0 || y != x + 2) {
EXPECT_FALSE(IsKnown(x, y, 4.f));
EXPECT_NEAR(0.f, GetIntensity(x, y, 4.f), 1e-6);
} else {
EXPECT_NEAR(options().hit_probability(), GetProbability(x, y, 4.f),
1e-4);
EXPECT_NEAR(10 + x, GetIntensity(x, y, 4.f), 1e-6);
}
}
}
}
TEST_F(RangeDataInserter3DTest, ProbabilityProgression) {
InsertPointCloud();
EXPECT_NEAR(options().hit_probability(), GetProbability(-2.f, 0.f, 4.f),

13
cartographer/mapping/3d/submap_3d.cc
View File

@ -202,6 +202,8 @@ Submap3D::Submap3D(const float high_resolution, const float low_resolution,
absl::make_unique<HybridGrid>(high_resolution)),
low_resolution_hybrid_grid_(
absl::make_unique<HybridGrid>(low_resolution)),
high_resolution_intensity_hybrid_grid_(
absl::make_unique<IntensityHybridGrid>(high_resolution)),
rotational_scan_matcher_histogram_(rotational_scan_matcher_histogram) {}
Submap3D::Submap3D(const proto::Submap3D& proto)
@ -278,9 +280,11 @@ void Submap3D::InsertData(const sensor::RangeData& range_data_in_local,
range_data_inserter.Insert(
FilterRangeDataByMaxRange(transformed_range_data,
high_resolution_max_range),
high_resolution_hybrid_grid_.get());
high_resolution_hybrid_grid_.get(),
high_resolution_intensity_hybrid_grid_.get());
range_data_inserter.Insert(transformed_range_data,
low_resolution_hybrid_grid_.get());
low_resolution_hybrid_grid_.get(),
/*intensity_hybrid_grid=*/nullptr);
set_num_range_data(num_range_data() + 1);
const float yaw_in_submap_from_gravity = transform::GetYaw(
local_pose().inverse().rotation() * local_from_gravity_aligned);
@ -332,6 +336,11 @@ void ActiveSubmaps3D::AddSubmap(
// This will crop the finished Submap before inserting a new Submap to
// reduce peak memory usage a bit.
CHECK(submaps_.front()->insertion_finished());
// We use `ForgetIntensityHybridGrid` to reduce memory usage. Since we use
// active submaps and their associated intensity hybrid grids for scan
// matching, we call `ForgetIntensityHybridGrid` once we remove the submap
// from active submaps and no longer need the intensity hybrid grid.
submaps_.front()->ForgetIntensityHybridGrid();
submaps_.erase(submaps_.begin());
}
const Eigen::VectorXf initial_rotational_scan_matcher_histogram =

9
cartographer/mapping/3d/submap_3d.h
View File

@ -60,6 +60,14 @@ class Submap3D : public Submap {
const HybridGrid& low_resolution_hybrid_grid() const {
return *low_resolution_hybrid_grid_;
}
const IntensityHybridGrid& high_resolution_intensity_hybrid_grid() const {
CHECK(high_resolution_intensity_hybrid_grid_ != nullptr);
return *high_resolution_intensity_hybrid_grid_;
}
void ForgetIntensityHybridGrid() {
high_resolution_intensity_hybrid_grid_.reset();
}
const Eigen::VectorXf& rotational_scan_matcher_histogram() const {
return rotational_scan_matcher_histogram_;
}
@ -79,6 +87,7 @@ class Submap3D : public Submap {
std::unique_ptr<HybridGrid> high_resolution_hybrid_grid_;
std::unique_ptr<HybridGrid> low_resolution_hybrid_grid_;
std::unique_ptr<IntensityHybridGrid> high_resolution_intensity_hybrid_grid_;
Eigen::VectorXf rotational_scan_matcher_histogram_;
};

1
cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc
View File

@ -129,6 +129,7 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
hit_probability = 0.7,
miss_probability = 0.4,
num_free_space_voxels = 0,
intensity_threshold = 100.0,
},
},
}

4
cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc
View File

@ -94,6 +94,7 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
"hit_probability = 0.7, "
"miss_probability = 0.4, "
"num_free_space_voxels = 5, "
"intensity_threshold = 100.0, "
"}");
return CreateRangeDataInserterOptions3D(parameter_dictionary.get());
}
@ -106,7 +107,8 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
sensor::RangeData{pose.translation(),
sensor::TransformPointCloud(point_cloud_, pose),
{}},
hybrid_grid_.get());
hybrid_grid_.get(),
/*intensity_hybrid_grid=*/nullptr);
hybrid_grid_->FinishUpdate();
return absl::make_unique<FastCorrelativeScanMatcher3D>(

3
cartographer/mapping/proto/range_data_inserter_options_3d.proto
View File

@ -28,4 +28,7 @@ message RangeDataInserterOptions3D {
// Up to how many free space voxels are updated for scan matching.
// 0 disables free space.
int32 num_free_space_voxels = 3;
// Do not insert intensities above this threshold into IntensityHybridGrid.
float intensity_threshold = 4;
}

2
configuration_files/trajectory_builder_3d.lua
View File

@ -13,6 +13,7 @@
-- limitations under the License.
MAX_3D_RANGE = 60.
INTENSITY_THRESHOLD = 40
TRAJECTORY_BUILDER_3D = {
min_range = 1.,
@ -96,6 +97,7 @@ TRAJECTORY_BUILDER_3D = {
hit_probability = 0.55,
miss_probability = 0.49,
num_free_space_voxels = 2,
intensity_threshold = INTENSITY_THRESHOLD,
},
},
}