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

Preparation for #1277: refactor rotation scan matcher (#1316) 

Added new constructor for `RotationalScanMatcher` and exposed `RotateHistogram`.

This PR prepares for PR #1277 where the constructor 
`RotationalScanMatcher(const std::vector<std::pair<Eigen::VectorXf, float>>& histograms_at_angles)`
will be removed and only the new constructor
`RotationalScanMatcher(const Eigen::VectorXf& histogram)`
will remain. The unit tests will be updated to use the new constructor in #1277.
master
Martin Schwörer 2018-07-23 19:02:17 +02:00 committed by Wally B. Feed
parent 25f79cb6eb
commit a42fd47146
3 changed files with 34 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc
View File

@ -117,10 +117,23 @@ sensor::PointCloud SortSlice(const sensor::PointCloud& slice) {
return result;
}
// Rotates the given 'histogram' by the given 'angle'. This might lead to
// rotations of a fractional bucket which is handled by linearly interpolating.
Eigen::VectorXf RotateHistogram(const Eigen::VectorXf& histogram,
const float angle) {
float MatchHistograms(const Eigen::VectorXf& submap_histogram,
const Eigen::VectorXf& scan_histogram) {
// We compute the dot product of normalized histograms as a measure of
// similarity.
const float scan_histogram_norm = scan_histogram.norm();
const float submap_histogram_norm = submap_histogram.norm();
const float normalization = scan_histogram_norm * submap_histogram_norm;
if (normalization < 1e-3f) {
return 1.f;
}
return submap_histogram.dot(scan_histogram) / normalization;
}
} // namespace
Eigen::VectorXf RotationalScanMatcher::RotateHistogram(
const Eigen::VectorXf& histogram, const float angle) {
const float rotate_by_buckets = -angle * histogram.size() / M_PI;
int full_buckets = common::RoundToInt(rotate_by_buckets - 0.5f);
const float fraction = rotate_by_buckets - full_buckets;
@ -138,21 +151,6 @@ Eigen::VectorXf RotateHistogram(const Eigen::VectorXf& histogram,
(1.f - fraction) * rotated_histogram_0;
}
float MatchHistograms(const Eigen::VectorXf& submap_histogram,
const Eigen::VectorXf& scan_histogram) {
// We compute the dot product of normalized histograms as a measure of
// similarity.
const float scan_histogram_norm = scan_histogram.norm();
const float submap_histogram_norm = submap_histogram.norm();
const float normalization = scan_histogram_norm * submap_histogram_norm;
if (normalization < 1e-3f) {
return 1.f;
}
return submap_histogram.dot(scan_histogram) / normalization;
}
} // namespace
Eigen::VectorXf RotationalScanMatcher::ComputeHistogram(
const sensor::PointCloud& point_cloud, const int histogram_size) {
Eigen::VectorXf histogram = Eigen::VectorXf::Zero(histogram_size);
@ -166,6 +164,9 @@ Eigen::VectorXf RotationalScanMatcher::ComputeHistogram(
return histogram;
}
RotationalScanMatcher::RotationalScanMatcher(const Eigen::VectorXf& histogram)
: histogram_(histogram) {}
RotationalScanMatcher::RotationalScanMatcher(
const std::vector<std::pair<Eigen::VectorXf, float>>& histograms_at_angles)
: histogram_(

8
cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h
View File

@ -28,10 +28,18 @@ namespace scan_matching {
class RotationalScanMatcher {
public:
// Rotates the given 'histogram' by the given 'angle'. This might lead to
// rotations of a fractional bucket which is handled by linearly
// interpolating.
static Eigen::VectorXf RotateHistogram(const Eigen::VectorXf& histogram,
float angle);
// Computes the histogram for a gravity aligned 'point_cloud'.
static Eigen::VectorXf ComputeHistogram(const sensor::PointCloud& point_cloud,
int histogram_size);
explicit RotationalScanMatcher(const Eigen::VectorXf& histogram);
// Creates a matcher from the given histograms rotated by the given angles.
// The angles should be chosen to bring the histograms into approximately the
// same frame.

9
cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher_test.cc
View File

@ -28,7 +28,9 @@ namespace {
TEST(RotationalScanMatcher3DTest, OnlySameHistogramIsScoreOne) {
Eigen::VectorXf histogram(7);
histogram << 1.f, 43.f, 0.5f, 0.3123f, 23.f, 42.f, 0.f;
RotationalScanMatcher matcher({{histogram, 0.f}});
const std::vector<std::pair<Eigen::VectorXf, float>> histogram_at_angle = {
{histogram, 0.f}};
RotationalScanMatcher matcher(histogram_at_angle);
const auto scores = matcher.Match(histogram, 0.f, {0.f, 1.f});
ASSERT_EQ(2, scores.size());
EXPECT_NEAR(1.f, scores[0], 1e-6);
@ -39,8 +41,9 @@ TEST(RotationalScanMatcher3DTest, InterpolatesAsExpected) {
constexpr int kNumBuckets = 10;
constexpr float kAnglePerBucket = M_PI / kNumBuckets;
constexpr float kNoInitialRotation = 0.f;
RotationalScanMatcher matcher(
{{Eigen::VectorXf::Unit(kNumBuckets, 3), kNoInitialRotation}});
const std::vector<std::pair<Eigen::VectorXf, float>> histogram_at_angle = {
{Eigen::VectorXf::Unit(kNumBuckets, 3), kNoInitialRotation}};
RotationalScanMatcher matcher(histogram_at_angle);
for (float t = 0.f; t < 1.f; t += 0.1f) {
// 't' is the fraction of overlap and we have to divide by the norm of the
// histogram to get the expected score.