Allow rotating histograms in the rotational scan matcher. (#501)

This allows reusing histograms to match for different yaws.
Adds a unit test to test that rotated histograms match as expected.
master
Wolfgang Hess 2017-09-06 10:59:16 +02:00 committed by GitHub
parent 18d8ea75fa
commit 4829ffee46
4 changed files with 163 additions and 62 deletions

View File

@ -125,6 +125,24 @@ struct Candidate {
bool operator>(const Candidate& other) const { return score > other.score; } bool operator>(const Candidate& other) const { return score > other.score; }
}; };
namespace {
scan_matching::RotationalScanMatcher CreateRotationalScanMatcher(
const std::vector<mapping::TrajectoryNode>& nodes,
const int histogram_size) {
Eigen::VectorXf histogram = Eigen::VectorXf::Zero(histogram_size);
for (const mapping::TrajectoryNode& node : nodes) {
histogram += scan_matching::RotationalScanMatcher::ComputeHistogram(
sensor::TransformPointCloud(
node.constant_data->range_data.returns.Decompress(),
node.pose.cast<float>()),
histogram_size);
}
return scan_matching::RotationalScanMatcher({{histogram, 0.f}});
}
} // namespace
FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
const HybridGrid& hybrid_grid, const HybridGrid& hybrid_grid,
const HybridGrid* const low_resolution_hybrid_grid, const HybridGrid* const low_resolution_hybrid_grid,
@ -136,7 +154,8 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
precomputation_grid_stack_( precomputation_grid_stack_(
common::make_unique<PrecomputationGridStack>(hybrid_grid, options)), common::make_unique<PrecomputationGridStack>(hybrid_grid, options)),
low_resolution_hybrid_grid_(low_resolution_hybrid_grid), low_resolution_hybrid_grid_(low_resolution_hybrid_grid),
rotational_scan_matcher_(nodes, options_.rotational_histogram_size()) {} rotational_scan_matcher_(CreateRotationalScanMatcher(
nodes, options_.rotational_histogram_size())) {}
FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {} FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}
@ -286,7 +305,10 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
angles.push_back(rz * angular_step_size); angles.push_back(rz * angular_step_size);
} }
const std::vector<float> scores = rotational_scan_matcher_.Match( const std::vector<float> scores = rotational_scan_matcher_.Match(
sensor::TransformPointCloud(fine_point_cloud, initial_pose), angles); RotationalScanMatcher::ComputeHistogram(
sensor::TransformPointCloud(fine_point_cloud, initial_pose),
options_.rotational_histogram_size()),
0.f /* initial_angle */, angles);
for (size_t i = 0; i != angles.size(); ++i) { for (size_t i = 0; i != angles.size(); ++i) {
if (scores[i] < options_.min_rotational_score()) { if (scores[i] < options_.min_rotational_score()) {
continue; continue;

View File

@ -20,6 +20,7 @@
#include <vector> #include <vector>
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/common/port.h"
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping_3d {
@ -57,14 +58,8 @@ Eigen::Vector3f ComputeCentroid(const sensor::PointCloud& slice) {
return sum / static_cast<float>(slice.size()); return sum / static_cast<float>(slice.size());
} }
struct AngleValuePair { void AddPointCloudSliceToHistogram(const sensor::PointCloud& slice,
float angle; Eigen::VectorXf* const histogram) {
float value;
};
void AddPointCloudSliceToValueVector(
const sensor::PointCloud& slice,
std::vector<AngleValuePair>* value_vector) {
if (slice.empty()) { if (slice.empty()) {
return; return;
} }
@ -88,7 +83,7 @@ void AddPointCloudSliceToValueVector(
const float angle = common::atan2(delta); const float angle = common::atan2(delta);
const float value = std::max( const float value = std::max(
0.f, 1.f - std::abs(delta.normalized().dot(direction.normalized()))); 0.f, 1.f - std::abs(delta.normalized().dot(direction.normalized())));
value_vector->push_back(AngleValuePair{angle, value}); AddValueToHistogram(angle, value, histogram);
} }
} }
@ -122,68 +117,78 @@ sensor::PointCloud SortSlice(const sensor::PointCloud& slice) {
return result; return result;
} }
std::vector<AngleValuePair> GetValuesForHistogram( // Rotates the given 'histogram' by the given 'angle'. This might lead to
const sensor::PointCloud& point_cloud) { // rotations of a fractional bucket which is handled by linearly interpolating.
std::map<int, sensor::PointCloud> slices; Eigen::VectorXf RotateHistogram(const Eigen::VectorXf& histogram,
for (const Eigen::Vector3f& point : point_cloud) { const float angle) {
slices[common::RoundToInt(point.z() / kSliceHeight)].push_back(point); 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;
while (full_buckets < 0) {
full_buckets += histogram.size();
} }
std::vector<AngleValuePair> result; Eigen::VectorXf rotated_histogram_0 = Eigen::VectorXf::Zero(histogram.size());
for (const auto& slice : slices) { Eigen::VectorXf rotated_histogram_1 = Eigen::VectorXf::Zero(histogram.size());
AddPointCloudSliceToValueVector(SortSlice(slice.second), &result); for (int i = 0; i != histogram.size(); ++i) {
rotated_histogram_0[i] = histogram[(i + full_buckets) % histogram.size()];
rotated_histogram_1[i] =
histogram[(i + 1 + full_buckets) % histogram.size()];
} }
return result; return fraction * rotated_histogram_1 +
(1.f - fraction) * rotated_histogram_0;
} }
void AddValuesToHistogram(const std::vector<AngleValuePair>& value_vector, float MatchHistograms(const Eigen::VectorXf& submap_histogram,
const float rotation, Eigen::VectorXf* histogram) { const Eigen::VectorXf& scan_histogram) {
for (const AngleValuePair& pair : value_vector) { // We compute the dot product of normalized histograms as a measure of
AddValueToHistogram(pair.angle + rotation, pair.value, histogram); // 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 } // namespace
Eigen::VectorXf RotationalScanMatcher::ComputeHistogram(
const sensor::PointCloud& point_cloud, const int histogram_size) {
Eigen::VectorXf histogram = Eigen::VectorXf::Zero(histogram_size);
std::map<int, sensor::PointCloud> slices;
for (const Eigen::Vector3f& point : point_cloud) {
slices[common::RoundToInt(point.z() / kSliceHeight)].push_back(point);
}
for (const auto& slice : slices) {
AddPointCloudSliceToHistogram(SortSlice(slice.second), &histogram);
}
return histogram;
}
RotationalScanMatcher::RotationalScanMatcher( RotationalScanMatcher::RotationalScanMatcher(
const std::vector<mapping::TrajectoryNode>& nodes, const int histogram_size) const std::vector<std::pair<Eigen::VectorXf, float>>& histograms_at_angles)
: histogram_(Eigen::VectorXf::Zero(histogram_size)) { : histogram_(
for (const mapping::TrajectoryNode& node : nodes) { Eigen::VectorXf::Zero(histograms_at_angles.at(0).first.size())) {
AddValuesToHistogram( for (const auto& histogram_at_angle : histograms_at_angles) {
GetValuesForHistogram(sensor::TransformPointCloud( histogram_ +=
node.constant_data->range_data.returns.Decompress(), RotateHistogram(histogram_at_angle.first, histogram_at_angle.second);
node.pose.cast<float>())),
0.f, &histogram_);
} }
} }
std::vector<float> RotationalScanMatcher::Match( std::vector<float> RotationalScanMatcher::Match(
const sensor::PointCloud& point_cloud, const Eigen::VectorXf& histogram, const float initial_angle,
const std::vector<float>& angles) const { const std::vector<float>& angles) const {
std::vector<float> result; std::vector<float> result;
result.reserve(angles.size()); result.reserve(angles.size());
const std::vector<AngleValuePair> value_vector =
GetValuesForHistogram(point_cloud);
for (const float angle : angles) { for (const float angle : angles) {
Eigen::VectorXf scan_histogram = Eigen::VectorXf::Zero(histogram_.size()); const Eigen::VectorXf scan_histogram =
AddValuesToHistogram(value_vector, angle, &scan_histogram); RotateHistogram(histogram, initial_angle + angle);
result.push_back(MatchHistogram(scan_histogram)); result.push_back(MatchHistograms(histogram_, scan_histogram));
} }
return result; return result;
} }
float RotationalScanMatcher::MatchHistogram(
const Eigen::VectorXf& scan_histogram) const {
// We compute the dot product of normalized histograms as a measure of
// similarity.
const float scan_histogram_norm = scan_histogram.norm();
const float histogram_norm = histogram_.norm();
const float normalization = scan_histogram_norm * histogram_norm;
if (normalization < 1e-3f) {
return 1.f;
}
return histogram_.dot(scan_histogram) / normalization;
}
} // namespace scan_matching } // namespace scan_matching
} // namespace mapping_3d } // namespace mapping_3d
} // namespace cartographer } // namespace cartographer

View File

@ -20,7 +20,6 @@
#include <vector> #include <vector>
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
namespace cartographer { namespace cartographer {
@ -29,21 +28,25 @@ namespace scan_matching {
class RotationalScanMatcher { class RotationalScanMatcher {
public: public:
// Computes the histogram for a gravity aligned 'point_cloud'.
static Eigen::VectorXf ComputeHistogram(const sensor::PointCloud& point_cloud,
int histogram_size);
// 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.
explicit RotationalScanMatcher( explicit RotationalScanMatcher(
const std::vector<mapping::TrajectoryNode>& nodes, int histogram_size); const std::vector<std::pair<Eigen::VectorXf, float>>&
histograms_at_angles);
RotationalScanMatcher(const RotationalScanMatcher&) = delete; // Scores how well 'histogram' rotated by 'initial_angle' can be understood as
RotationalScanMatcher& operator=(const RotationalScanMatcher&) = delete; // further rotated by certain 'angles' relative to the 'nodes'. Each angle
// results in a score between 0 (worst) and 1 (best).
// Scores how well a 'point_cloud' can be understood as rotated by certain std::vector<float> Match(const Eigen::VectorXf& histogram,
// 'angles' relative to the 'nodes'. Each angle results in a score between float initial_angle,
// 0 (worst) and 1 (best).
std::vector<float> Match(const sensor::PointCloud& point_cloud,
const std::vector<float>& angles) const; const std::vector<float>& angles) const;
private: private:
float MatchHistogram(const Eigen::VectorXf& scan_histogram) const;
Eigen::VectorXf histogram_; Eigen::VectorXf histogram_;
}; };

View File

@ -0,0 +1,71 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h"
#include <cmath>
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping_3d {
namespace scan_matching {
namespace {
TEST(RotationalScanMatcherTest, 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 auto scores = matcher.Match(histogram, 0.f, {0.f, 1.f});
ASSERT_EQ(2, scores.size());
EXPECT_NEAR(1.f, scores[0], 1e-6);
EXPECT_GT(1.f, scores[1]);
}
TEST(RotationalScanMatcherTest, InterpolatesAsExpected) {
constexpr int kNumBuckets = 10;
constexpr float kAnglePerBucket = M_PI / kNumBuckets;
constexpr float kNoInitialRotation = 0.f;
RotationalScanMatcher matcher(
{{Eigen::VectorXf::Unit(kNumBuckets, 3), kNoInitialRotation}});
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.
const float expected_score = t / std::hypot(t, 1 - t);
// We rotate the 't'-th fraction of a bucket into the matcher's histogram.
auto scores = matcher.Match(Eigen::VectorXf::Unit(kNumBuckets, 2),
kNoInitialRotation, {t * kAnglePerBucket});
ASSERT_EQ(1, scores.size());
EXPECT_NEAR(expected_score, scores[0], 1e-6);
// Also verify rotating out of a bucket.
scores = matcher.Match(Eigen::VectorXf::Unit(kNumBuckets, 2),
kNoInitialRotation, {(2 - t) * kAnglePerBucket});
ASSERT_EQ(1, scores.size());
EXPECT_NEAR(expected_score, scores[0], 1e-6);
// And into and out of a bucket with negative angle.
scores =
matcher.Match(Eigen::VectorXf::Unit(kNumBuckets, 4), kNoInitialRotation,
{-t * kAnglePerBucket, (t - 2) * kAnglePerBucket});
ASSERT_EQ(2, scores.size());
EXPECT_NEAR(expected_score, scores[0], 1e-6);
EXPECT_NEAR(expected_score, scores[1], 1e-6);
}
}
} // namespace
} // namespace scan_matching
} // namespace mapping_3d
} // namespace cartographer