Store histogram in submap (#1277)

The histogram of a submap is now stored in the submap (class and proto) itself. This change allows to accumulate the histogram of a submap in local SLAM by adding up the histogram of each new scan.

The main advantage is that the background thread doesn't have to loop over all `TrajectoryNode`s of a finished submap to compute the submap histogram for the `RotationalScanMatcher`. Instead this chunk of work is moved to the local SLAM thread but is split up into a few computations for each new scan. When running localization, the histogram of a submap can just be read from a map pbstream and does not have to be computed from the nodes.

In summary:
- This change improved the CPU time of offline SLAM by ~7%.
- Increases the readability of the code and performance of the background thread. (see `PoseGraph3D::ComputeConstraint`)
- No negative performance impacts on accuracy or finding loop-closures

However:
- With this change to the submap proto, old maps (pbstreams) are no longer supported and need to be re-created by running offline slam
master
Martin Schwörer 2018-09-04 12:02:57 +02:00 committed by Wally B. Feed
parent b1dfa30ee3
commit 3b511aa1ba
16 changed files with 109 additions and 133 deletions

View File

@ -25,9 +25,8 @@ namespace cartographer {
namespace io { namespace io {
// The current serialization format version. // The current serialization format version.
static constexpr int kMappingStateSerializationFormatVersion = 1; static constexpr int kMappingStateSerializationFormatVersion = 2;
static constexpr int kFormatVersionWithoutSubmapHistograms = static constexpr int kFormatVersionWithoutSubmapHistograms = 1;
kMappingStateSerializationFormatVersion;
// Serialize mapping state to a pbstream. // Serialize mapping state to a pbstream.
void WritePbStream( void WritePbStream(

View File

@ -31,7 +31,8 @@ mapping::proto::SerializationHeader ReadHeaderOrDie(
} }
bool IsVersionSupported(const mapping::proto::SerializationHeader& header) { bool IsVersionSupported(const mapping::proto::SerializationHeader& header) {
return header.format_version() == kMappingStateSerializationFormatVersion; return header.format_version() == kMappingStateSerializationFormatVersion ||
header.format_version() == kFormatVersionWithoutSubmapHistograms;
} }
} // namespace } // namespace

View File

@ -197,12 +197,14 @@ proto::SubmapsOptions3D CreateSubmapsOptions3D(
} }
Submap3D::Submap3D(const float high_resolution, const float low_resolution, Submap3D::Submap3D(const float high_resolution, const float low_resolution,
const transform::Rigid3d& local_submap_pose) const transform::Rigid3d& local_submap_pose,
const Eigen::VectorXf& rotational_scan_matcher_histogram)
: Submap(local_submap_pose), : Submap(local_submap_pose),
high_resolution_hybrid_grid_( high_resolution_hybrid_grid_(
absl::make_unique<HybridGrid>(high_resolution)), absl::make_unique<HybridGrid>(high_resolution)),
low_resolution_hybrid_grid_( low_resolution_hybrid_grid_(
absl::make_unique<HybridGrid>(low_resolution)) {} absl::make_unique<HybridGrid>(low_resolution)),
rotational_scan_matcher_histogram_(rotational_scan_matcher_histogram) {}
Submap3D::Submap3D(const proto::Submap3D& proto) Submap3D::Submap3D(const proto::Submap3D& proto)
: Submap(transform::ToRigid3(proto.local_pose())) { : Submap(transform::ToRigid3(proto.local_pose())) {
@ -266,9 +268,11 @@ void Submap3D::ToResponseProto(
response->add_textures()); response->add_textures());
} }
void Submap3D::InsertRangeData(const sensor::RangeData& range_data_in_local, void Submap3D::InsertData(const sensor::RangeData& range_data_in_local,
const RangeDataInserter3D& range_data_inserter, const RangeDataInserter3D& range_data_inserter,
const float high_resolution_max_range) { const float high_resolution_max_range,
const Eigen::Quaterniond& local_from_gravity_aligned,
const Eigen::VectorXf& scan_histogram_in_gravity) {
CHECK(!insertion_finished()); CHECK(!insertion_finished());
// Transform range data into submap frame. // Transform range data into submap frame.
const sensor::RangeData transformed_range_data = sensor::TransformRangeData( const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
@ -280,6 +284,11 @@ void Submap3D::InsertRangeData(const sensor::RangeData& range_data_in_local,
range_data_inserter.Insert(transformed_range_data, range_data_inserter.Insert(transformed_range_data,
low_resolution_hybrid_grid_.get()); low_resolution_hybrid_grid_.get());
set_num_range_data(num_range_data() + 1); 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);
rotational_scan_matcher_histogram_ +=
scan_matching::RotationalScanMatcher::RotateHistogram(
scan_histogram_in_gravity, yaw_in_submap_from_gravity);
} }
void Submap3D::Finish() { void Submap3D::Finish() {
@ -296,17 +305,21 @@ std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::submaps() const {
submaps_.end()); submaps_.end());
} }
std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::InsertRangeData( std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::InsertData(
const sensor::RangeData& range_data, const sensor::RangeData& range_data,
const Eigen::Quaterniond& local_from_gravity_aligned) { const Eigen::Quaterniond& local_from_gravity_aligned,
const Eigen::VectorXf& rotational_scan_matcher_histogram_in_gravity) {
if (submaps_.empty() || if (submaps_.empty() ||
submaps_.back()->num_range_data() == options_.num_range_data()) { submaps_.back()->num_range_data() == options_.num_range_data()) {
AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(), AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
local_from_gravity_aligned)); local_from_gravity_aligned),
rotational_scan_matcher_histogram_in_gravity.size());
} }
for (auto& submap : submaps_) { for (auto& submap : submaps_) {
submap->InsertRangeData(range_data, range_data_inserter_, submap->InsertData(range_data, range_data_inserter_,
options_.high_resolution_max_range()); options_.high_resolution_max_range(),
local_from_gravity_aligned,
rotational_scan_matcher_histogram_in_gravity);
} }
if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) { if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {
submaps_.front()->Finish(); submaps_.front()->Finish();
@ -314,16 +327,20 @@ std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::InsertRangeData(
return submaps(); return submaps();
} }
void ActiveSubmaps3D::AddSubmap(const transform::Rigid3d& local_submap_pose) { void ActiveSubmaps3D::AddSubmap(
const transform::Rigid3d& local_submap_pose,
const int rotational_scan_matcher_histogram_size) {
if (submaps_.size() >= 2) { if (submaps_.size() >= 2) {
// This will crop the finished Submap before inserting a new Submap to // This will crop the finished Submap before inserting a new Submap to
// reduce peak memory usage a bit. // reduce peak memory usage a bit.
CHECK(submaps_.front()->insertion_finished()); CHECK(submaps_.front()->insertion_finished());
submaps_.erase(submaps_.begin()); submaps_.erase(submaps_.begin());
} }
submaps_.emplace_back(new Submap3D(options_.high_resolution(), const Eigen::VectorXf initial_rotational_scan_matcher_histogram =
options_.low_resolution(), Eigen::VectorXf::Zero(rotational_scan_matcher_histogram_size);
local_submap_pose)); submaps_.emplace_back(new Submap3D(
options_.high_resolution(), options_.low_resolution(), local_submap_pose,
initial_rotational_scan_matcher_histogram));
} }
} // namespace mapping } // namespace mapping

View File

@ -43,7 +43,9 @@ proto::SubmapsOptions3D CreateSubmapsOptions3D(
class Submap3D : public Submap { class Submap3D : public Submap {
public: public:
Submap3D(float high_resolution, float low_resolution, Submap3D(float high_resolution, float low_resolution,
const transform::Rigid3d& local_submap_pose); const transform::Rigid3d& local_submap_pose,
const Eigen::VectorXf& rotational_scan_matcher_histogram);
explicit Submap3D(const proto::Submap3D& proto); explicit Submap3D(const proto::Submap3D& proto);
proto::Submap ToProto(bool include_probability_grid_data) const override; proto::Submap ToProto(bool include_probability_grid_data) const override;
@ -64,9 +66,12 @@ class Submap3D : public Submap {
// Insert 'range_data' into this submap using 'range_data_inserter'. The // Insert 'range_data' into this submap using 'range_data_inserter'. The
// submap must not be finished yet. // submap must not be finished yet.
void InsertRangeData(const sensor::RangeData& range_data, void InsertData(const sensor::RangeData& range_data,
const RangeDataInserter3D& range_data_inserter, const RangeDataInserter3D& range_data_inserter,
float high_resolution_max_range); float high_resolution_max_range,
const Eigen::Quaterniond& local_from_gravity_aligned,
const Eigen::VectorXf& scan_histogram_in_gravity);
void Finish(); void Finish();
private: private:
@ -97,14 +102,18 @@ class ActiveSubmaps3D {
// Inserts 'range_data_in_local' into the Submap collection. // Inserts 'range_data_in_local' into the Submap collection.
// 'local_from_gravity_aligned' is used for the orientation of new submaps so // 'local_from_gravity_aligned' is used for the orientation of new submaps so
// that the z axis approximately aligns with gravity. // that the z axis approximately aligns with gravity.
std::vector<std::shared_ptr<const Submap3D>> InsertRangeData( // 'rotational_scan_matcher_histogram_in_gravity' will be accumulated in all
// submaps of the Submap collection.
std::vector<std::shared_ptr<const Submap3D>> InsertData(
const sensor::RangeData& range_data_in_local, const sensor::RangeData& range_data_in_local,
const Eigen::Quaterniond& local_from_gravity_aligned); const Eigen::Quaterniond& local_from_gravity_aligned,
const Eigen::VectorXf& rotational_scan_matcher_histogram_in_gravity);
std::vector<std::shared_ptr<const Submap3D>> submaps() const; std::vector<std::shared_ptr<const Submap3D>> submaps() const;
private: private:
void AddSubmap(const transform::Rigid3d& local_submap_pose); void AddSubmap(const transform::Rigid3d& local_submap_pose,
int rotational_scan_matcher_histogram_size);
const proto::SubmapsOptions3D options_; const proto::SubmapsOptions3D options_;
std::vector<std::shared_ptr<Submap3D>> submaps_; std::vector<std::shared_ptr<Submap3D>> submaps_;

View File

@ -24,10 +24,13 @@ namespace mapping {
namespace { namespace {
TEST(SubmapsTest, ToFromProto) { TEST(SubmapsTest, ToFromProto) {
Eigen::VectorXf histogram(2);
histogram << 1.0f, 2.0f;
const Submap3D expected( const Submap3D expected(
0.05, 0.25, 0.05, 0.25,
transform::Rigid3d(Eigen::Vector3d(1., 2., 0.), transform::Rigid3d(Eigen::Vector3d(1., 2., 0.),
Eigen::Quaterniond(0., 0., 0., 1.))); Eigen::Quaterniond(0., 0., 0., 1.)),
histogram);
const proto::Submap proto = const proto::Submap proto =
expected.ToProto(true /* include_probability_grid_data */); expected.ToProto(true /* include_probability_grid_data */);
EXPECT_FALSE(proto.has_submap_2d()); EXPECT_FALSE(proto.has_submap_2d());
@ -41,6 +44,8 @@ TEST(SubmapsTest, ToFromProto) {
EXPECT_EQ(expected.insertion_finished(), actual.insertion_finished()); EXPECT_EQ(expected.insertion_finished(), actual.insertion_finished());
EXPECT_NEAR(expected.high_resolution_hybrid_grid().resolution(), 0.05, 1e-6); EXPECT_NEAR(expected.high_resolution_hybrid_grid().resolution(), 0.05, 1e-6);
EXPECT_NEAR(expected.low_resolution_hybrid_grid().resolution(), 0.25, 1e-6); EXPECT_NEAR(expected.low_resolution_hybrid_grid().resolution(), 0.25, 1e-6);
EXPECT_TRUE(expected.rotational_scan_matcher_histogram().isApprox(
actual.rotational_scan_matcher_histogram(), 1e-6));
} }
} // namespace } // namespace

View File

@ -352,19 +352,19 @@ LocalTrajectoryBuilder3D::InsertIntoSubmap(
if (motion_filter_.IsSimilar(time, pose_estimate)) { if (motion_filter_.IsSimilar(time, pose_estimate)) {
return nullptr; return nullptr;
} }
const auto local_from_gravity_aligned = const Eigen::VectorXf rotational_scan_matcher_histogram_in_gravity =
pose_estimate.rotation() * gravity_alignment.inverse();
std::vector<std::shared_ptr<const mapping::Submap3D>> insertion_submaps =
active_submaps_.InsertRangeData(filtered_range_data_in_local,
local_from_gravity_aligned);
const Eigen::VectorXf rotational_scan_matcher_histogram =
scan_matching::RotationalScanMatcher::ComputeHistogram( scan_matching::RotationalScanMatcher::ComputeHistogram(
sensor::TransformPointCloud( sensor::TransformPointCloud(
filtered_range_data_in_tracking.returns, filtered_range_data_in_tracking.returns,
transform::Rigid3f::Rotation(gravity_alignment.cast<float>())), transform::Rigid3f::Rotation(gravity_alignment.cast<float>())),
options_.rotational_histogram_size()); options_.rotational_histogram_size());
const Eigen::Quaterniond local_from_gravity_aligned =
pose_estimate.rotation() * gravity_alignment.inverse();
std::vector<std::shared_ptr<const mapping::Submap3D>> insertion_submaps =
active_submaps_.InsertData(filtered_range_data_in_local,
local_from_gravity_aligned,
rotational_scan_matcher_histogram_in_gravity);
return absl::make_unique<InsertionResult>( return absl::make_unique<InsertionResult>(
InsertionResult{std::make_shared<const mapping::TrajectoryNode::Data>( InsertionResult{std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{ mapping::TrajectoryNode::Data{
@ -373,7 +373,7 @@ LocalTrajectoryBuilder3D::InsertIntoSubmap(
{}, // 'filtered_point_cloud' is only used in 2D. {}, // 'filtered_point_cloud' is only used in 2D.
high_resolution_point_cloud_in_tracking, high_resolution_point_cloud_in_tracking,
low_resolution_point_cloud_in_tracking, low_resolution_point_cloud_in_tracking,
rotational_scan_matcher_histogram, rotational_scan_matcher_histogram_in_gravity,
pose_estimate}), pose_estimate}),
std::move(insertion_submaps)}); std::move(insertion_submaps)});
} }

View File

@ -246,14 +246,10 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
const transform::Rigid3d global_submap_pose = const transform::Rigid3d global_submap_pose =
optimization_problem_->submap_data().at(submap_id).global_pose; optimization_problem_->submap_data().at(submap_id).global_pose;
const transform::Rigid3d global_submap_pose_inverse =
global_submap_pose.inverse();
bool maybe_add_local_constraint = false; bool maybe_add_local_constraint = false;
bool maybe_add_global_constraint = false; bool maybe_add_global_constraint = false;
const TrajectoryNode::Data* constant_data; const TrajectoryNode::Data* constant_data;
const Submap3D* submap; const Submap3D* submap;
std::vector<TrajectoryNode> submap_nodes;
{ {
absl::MutexLock locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished); CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
@ -263,14 +259,6 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
return; return;
} }
for (const NodeId& submap_node_id :
data_.submap_data.at(submap_id).node_ids) {
submap_nodes.push_back(TrajectoryNode{
data_.trajectory_nodes.at(submap_node_id).constant_data,
global_submap_pose_inverse *
data_.trajectory_nodes.at(submap_node_id).global_pose});
}
const common::Time node_time = GetLatestNodeTime(node_id, submap_id); const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
const common::Time last_connection_time = const common::Time last_connection_time =
data_.trajectory_connectivity_state.LastConnectionTime( data_.trajectory_connectivity_state.LastConnectionTime(
@ -299,13 +287,13 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
} }
if (maybe_add_local_constraint) { if (maybe_add_local_constraint) {
constraint_builder_.MaybeAddConstraint( constraint_builder_.MaybeAddConstraint(submap_id, submap, node_id,
submap_id, submap, node_id, constant_data, submap_nodes, constant_data, global_node_pose,
global_node_pose, global_submap_pose); global_submap_pose);
} else if (maybe_add_global_constraint) { } else if (maybe_add_global_constraint) {
constraint_builder_.MaybeAddGlobalConstraint( constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap, node_id, constant_data, submap_nodes, submap_id, submap, node_id, constant_data, global_node_pose.rotation(),
global_node_pose.rotation(), global_submap_pose.rotation()); global_submap_pose.rotation());
} }
} }

View File

@ -109,28 +109,10 @@ struct Candidate3D {
bool operator>(const Candidate3D& other) const { return score > other.score; } bool operator>(const Candidate3D& other) const { return score > other.score; }
}; };
namespace {
std::vector<std::pair<Eigen::VectorXf, float>> HistogramsAtAnglesFromNodes(
const std::vector<TrajectoryNode>& nodes) {
std::vector<std::pair<Eigen::VectorXf, float>> histograms_at_angles;
for (const auto& node : nodes) {
histograms_at_angles.emplace_back(
node.constant_data->rotational_scan_matcher_histogram,
transform::GetYaw(
node.global_pose *
transform::Rigid3d::Rotation(
node.constant_data->gravity_alignment.inverse())));
}
return histograms_at_angles;
}
} // namespace
FastCorrelativeScanMatcher3D::FastCorrelativeScanMatcher3D( FastCorrelativeScanMatcher3D::FastCorrelativeScanMatcher3D(
const HybridGrid& hybrid_grid, const HybridGrid& hybrid_grid,
const HybridGrid* const low_resolution_hybrid_grid, const HybridGrid* const low_resolution_hybrid_grid,
const std::vector<TrajectoryNode>& nodes, const Eigen::VectorXf* rotational_scan_matcher_histogram,
const proto::FastCorrelativeScanMatcherOptions3D& options) const proto::FastCorrelativeScanMatcherOptions3D& options)
: options_(options), : options_(options),
resolution_(hybrid_grid.resolution()), resolution_(hybrid_grid.resolution()),
@ -138,7 +120,7 @@ FastCorrelativeScanMatcher3D::FastCorrelativeScanMatcher3D(
precomputation_grid_stack_( precomputation_grid_stack_(
absl::make_unique<PrecomputationGridStack3D>(hybrid_grid, options)), absl::make_unique<PrecomputationGridStack3D>(hybrid_grid, options)),
low_resolution_hybrid_grid_(low_resolution_hybrid_grid), low_resolution_hybrid_grid_(low_resolution_hybrid_grid),
rotational_scan_matcher_(HistogramsAtAnglesFromNodes(nodes)) {} rotational_scan_matcher_(rotational_scan_matcher_histogram) {}
FastCorrelativeScanMatcher3D::~FastCorrelativeScanMatcher3D() {} FastCorrelativeScanMatcher3D::~FastCorrelativeScanMatcher3D() {}

View File

@ -75,7 +75,7 @@ class FastCorrelativeScanMatcher3D {
FastCorrelativeScanMatcher3D( FastCorrelativeScanMatcher3D(
const HybridGrid& hybrid_grid, const HybridGrid& hybrid_grid,
const HybridGrid* low_resolution_hybrid_grid, const HybridGrid* low_resolution_hybrid_grid,
const std::vector<TrajectoryNode>& nodes, const Eigen::VectorXf* rotational_scan_matcher_histogram,
const proto::FastCorrelativeScanMatcherOptions3D& options); const proto::FastCorrelativeScanMatcherOptions3D& options);
~FastCorrelativeScanMatcher3D(); ~FastCorrelativeScanMatcher3D();

View File

@ -105,11 +105,7 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
hybrid_grid_->FinishUpdate(); hybrid_grid_->FinishUpdate();
return absl::make_unique<FastCorrelativeScanMatcher3D>( return absl::make_unique<FastCorrelativeScanMatcher3D>(
*hybrid_grid_, hybrid_grid_.get(), *hybrid_grid_, hybrid_grid_.get(), &GetRotationalScanMatcherHistogram(),
std::vector<TrajectoryNode>(
{{std::make_shared<const TrajectoryNode::Data>(
CreateConstantData(point_cloud_)),
pose.cast<double>()}}),
options); options);
} }
@ -120,7 +116,11 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
{}, {},
point_cloud_, point_cloud_,
low_resolution_point_cloud, low_resolution_point_cloud,
Eigen::VectorXf::Zero(10)}; GetRotationalScanMatcherHistogram()};
}
const Eigen::VectorXf& GetRotationalScanMatcherHistogram() {
return rotational_scan_matcher_histogram_;
} }
std::mt19937 prng_ = std::mt19937(42); std::mt19937 prng_ = std::mt19937(42);
@ -130,6 +130,8 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
const proto::FastCorrelativeScanMatcherOptions3D options_; const proto::FastCorrelativeScanMatcherOptions3D options_;
sensor::PointCloud point_cloud_; sensor::PointCloud point_cloud_;
std::unique_ptr<HybridGrid> hybrid_grid_; std::unique_ptr<HybridGrid> hybrid_grid_;
const Eigen::VectorXf rotational_scan_matcher_histogram_ =
Eigen::VectorXf::Zero(10);
}; };
constexpr float kMinScore = 0.1f; constexpr float kMinScore = 0.1f;

View File

@ -133,6 +133,11 @@ float MatchHistograms(const Eigen::VectorXf& submap_histogram,
} // namespace } // namespace
RotationalScanMatcher::RotationalScanMatcher(const Eigen::VectorXf* histogram)
: histogram_(histogram) {}
// 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 RotationalScanMatcher::RotateHistogram( Eigen::VectorXf RotationalScanMatcher::RotateHistogram(
const Eigen::VectorXf& histogram, const float angle) { const Eigen::VectorXf& histogram, const float angle) {
const float rotate_by_buckets = -angle * histogram.size() / M_PI; const float rotate_by_buckets = -angle * histogram.size() / M_PI;
@ -166,19 +171,6 @@ Eigen::VectorXf RotationalScanMatcher::ComputeHistogram(
return histogram; return histogram;
} }
RotationalScanMatcher::RotationalScanMatcher(const Eigen::VectorXf& histogram)
: histogram_(histogram) {}
RotationalScanMatcher::RotationalScanMatcher(
const std::vector<std::pair<Eigen::VectorXf, float>>& histograms_at_angles)
: histogram_(
Eigen::VectorXf::Zero(histograms_at_angles.at(0).first.size())) {
for (const auto& histogram_at_angle : histograms_at_angles) {
histogram_ +=
RotateHistogram(histogram_at_angle.first, histogram_at_angle.second);
}
}
std::vector<float> RotationalScanMatcher::Match( std::vector<float> RotationalScanMatcher::Match(
const Eigen::VectorXf& histogram, const float initial_angle, const Eigen::VectorXf& histogram, const float initial_angle,
const std::vector<float>& angles) const { const std::vector<float>& angles) const {
@ -187,7 +179,7 @@ std::vector<float> RotationalScanMatcher::Match(
for (const float angle : angles) { for (const float angle : angles) {
const Eigen::VectorXf scan_histogram = const Eigen::VectorXf scan_histogram =
RotateHistogram(histogram, initial_angle + angle); RotateHistogram(histogram, initial_angle + angle);
result.push_back(MatchHistograms(histogram_, scan_histogram)); result.push_back(MatchHistograms(*histogram_, scan_histogram));
} }
return result; return result;
} }

View File

@ -38,14 +38,7 @@ class RotationalScanMatcher {
static Eigen::VectorXf ComputeHistogram(const sensor::PointCloud& point_cloud, static Eigen::VectorXf ComputeHistogram(const sensor::PointCloud& point_cloud,
int histogram_size); int histogram_size);
explicit RotationalScanMatcher(const Eigen::VectorXf& histogram); 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.
explicit RotationalScanMatcher(
const std::vector<std::pair<Eigen::VectorXf, float>>&
histograms_at_angles);
// Scores how well 'histogram' rotated by 'initial_angle' can be understood as // Scores how well 'histogram' rotated by 'initial_angle' can be understood as
// further rotated by certain 'angles' relative to the 'nodes'. Each angle // further rotated by certain 'angles' relative to the 'nodes'. Each angle
@ -55,7 +48,7 @@ class RotationalScanMatcher {
const std::vector<float>& angles) const; const std::vector<float>& angles) const;
private: private:
Eigen::VectorXf histogram_; const Eigen::VectorXf* histogram_;
}; };
} // namespace scan_matching } // namespace scan_matching

View File

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

View File

@ -77,7 +77,6 @@ ConstraintBuilder3D::~ConstraintBuilder3D() {
void ConstraintBuilder3D::MaybeAddConstraint( void ConstraintBuilder3D::MaybeAddConstraint(
const SubmapId& submap_id, const Submap3D* const submap, const SubmapId& submap_id, const Submap3D* const submap,
const NodeId& node_id, const TrajectoryNode::Data* const constant_data, const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
const std::vector<TrajectoryNode>& submap_nodes,
const transform::Rigid3d& global_node_pose, const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose) { const transform::Rigid3d& global_submap_pose) {
if ((global_node_pose.translation() - global_submap_pose.translation()) if ((global_node_pose.translation() - global_submap_pose.translation())
@ -94,8 +93,7 @@ void ConstraintBuilder3D::MaybeAddConstraint(
constraints_.emplace_back(); constraints_.emplace_back();
kQueueLengthMetric->Set(constraints_.size()); kQueueLengthMetric->Set(constraints_.size());
auto* const constraint = &constraints_.back(); auto* const constraint = &constraints_.back();
const auto* scan_matcher = const auto* scan_matcher = DispatchScanMatcherConstruction(submap_id, submap);
DispatchScanMatcherConstruction(submap_id, submap_nodes, submap);
auto constraint_task = absl::make_unique<common::Task>(); auto constraint_task = absl::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) { constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
ComputeConstraint(submap_id, node_id, false, /* match_full_submap */ ComputeConstraint(submap_id, node_id, false, /* match_full_submap */
@ -111,7 +109,6 @@ void ConstraintBuilder3D::MaybeAddConstraint(
void ConstraintBuilder3D::MaybeAddGlobalConstraint( void ConstraintBuilder3D::MaybeAddGlobalConstraint(
const SubmapId& submap_id, const Submap3D* const submap, const SubmapId& submap_id, const Submap3D* const submap,
const NodeId& node_id, const TrajectoryNode::Data* const constant_data, const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
const std::vector<TrajectoryNode>& submap_nodes,
const Eigen::Quaterniond& global_node_rotation, const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation) { const Eigen::Quaterniond& global_submap_rotation) {
absl::MutexLock locker(&mutex_); absl::MutexLock locker(&mutex_);
@ -122,8 +119,7 @@ void ConstraintBuilder3D::MaybeAddGlobalConstraint(
constraints_.emplace_back(); constraints_.emplace_back();
kQueueLengthMetric->Set(constraints_.size()); kQueueLengthMetric->Set(constraints_.size());
auto* const constraint = &constraints_.back(); auto* const constraint = &constraints_.back();
const auto* scan_matcher = const auto* scan_matcher = DispatchScanMatcherConstruction(submap_id, submap);
DispatchScanMatcherConstruction(submap_id, submap_nodes, submap);
auto constraint_task = absl::make_unique<common::Task>(); auto constraint_task = absl::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) { constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
ComputeConstraint(submap_id, node_id, true, /* match_full_submap */ ComputeConstraint(submap_id, node_id, true, /* match_full_submap */
@ -165,9 +161,8 @@ void ConstraintBuilder3D::WhenDone(
} }
const ConstraintBuilder3D::SubmapScanMatcher* const ConstraintBuilder3D::SubmapScanMatcher*
ConstraintBuilder3D::DispatchScanMatcherConstruction( ConstraintBuilder3D::DispatchScanMatcherConstruction(const SubmapId& submap_id,
const SubmapId& submap_id, const std::vector<TrajectoryNode>& submap_nodes, const Submap3D* submap) {
const Submap3D* submap) {
if (submap_scan_matchers_.count(submap_id) != 0) { if (submap_scan_matchers_.count(submap_id) != 0) {
return &submap_scan_matchers_.at(submap_id); return &submap_scan_matchers_.at(submap_id);
} }
@ -178,13 +173,15 @@ ConstraintBuilder3D::DispatchScanMatcherConstruction(
&submap->low_resolution_hybrid_grid(); &submap->low_resolution_hybrid_grid();
auto& scan_matcher_options = auto& scan_matcher_options =
options_.fast_correlative_scan_matcher_options_3d(); options_.fast_correlative_scan_matcher_options_3d();
const Eigen::VectorXf* histogram =
&submap->rotational_scan_matcher_histogram();
auto scan_matcher_task = absl::make_unique<common::Task>(); auto scan_matcher_task = absl::make_unique<common::Task>();
scan_matcher_task->SetWorkItem( scan_matcher_task->SetWorkItem(
[&submap_scan_matcher, &scan_matcher_options, submap_nodes]() { [&submap_scan_matcher, &scan_matcher_options, histogram]() {
submap_scan_matcher.fast_correlative_scan_matcher = submap_scan_matcher.fast_correlative_scan_matcher =
absl::make_unique<scan_matching::FastCorrelativeScanMatcher3D>( absl::make_unique<scan_matching::FastCorrelativeScanMatcher3D>(
*submap_scan_matcher.high_resolution_hybrid_grid, *submap_scan_matcher.high_resolution_hybrid_grid,
submap_scan_matcher.low_resolution_hybrid_grid, submap_nodes, submap_scan_matcher.low_resolution_hybrid_grid, histogram,
scan_matcher_options); scan_matcher_options);
}); });
submap_scan_matcher.creation_task_handle = submap_scan_matcher.creation_task_handle =

View File

@ -78,7 +78,6 @@ class ConstraintBuilder3D {
void MaybeAddConstraint(const SubmapId& submap_id, const Submap3D* submap, void MaybeAddConstraint(const SubmapId& submap_id, const Submap3D* submap,
const NodeId& node_id, const NodeId& node_id,
const TrajectoryNode::Data* const constant_data, const TrajectoryNode::Data* const constant_data,
const std::vector<TrajectoryNode>& submap_nodes,
const transform::Rigid3d& global_node_pose, const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose); const transform::Rigid3d& global_submap_pose);
@ -94,7 +93,6 @@ class ConstraintBuilder3D {
void MaybeAddGlobalConstraint( void MaybeAddGlobalConstraint(
const SubmapId& submap_id, const Submap3D* submap, const NodeId& node_id, const SubmapId& submap_id, const Submap3D* submap, const NodeId& node_id,
const TrajectoryNode::Data* const constant_data, const TrajectoryNode::Data* const constant_data,
const std::vector<TrajectoryNode>& submap_nodes,
const Eigen::Quaterniond& global_node_rotation, const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation); const Eigen::Quaterniond& global_submap_rotation);
@ -126,8 +124,7 @@ class ConstraintBuilder3D {
// The returned 'grid' and 'fast_correlative_scan_matcher' must only be // The returned 'grid' and 'fast_correlative_scan_matcher' must only be
// accessed after 'creation_task_handle' has completed. // accessed after 'creation_task_handle' has completed.
const SubmapScanMatcher* DispatchScanMatcherConstruction( const SubmapScanMatcher* DispatchScanMatcherConstruction(
const SubmapId& submap_id, const SubmapId& submap_id, const Submap3D* submap)
const std::vector<TrajectoryNode>& submap_nodes, const Submap3D* submap)
EXCLUSIVE_LOCKS_REQUIRED(mutex_); EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Runs in a background thread and does computations for an additional // Runs in a background thread and does computations for an additional

View File

@ -71,7 +71,6 @@ TEST_F(ConstraintBuilder3DTest, CallsBack) {
} }
TEST_F(ConstraintBuilder3DTest, FindsConstraints) { TEST_F(ConstraintBuilder3DTest, FindsConstraints) {
TrajectoryNode node;
auto node_data = std::make_shared<TrajectoryNode::Data>(); auto node_data = std::make_shared<TrajectoryNode::Data>();
node_data->gravity_alignment = Eigen::Quaterniond::Identity(); node_data->gravity_alignment = Eigen::Quaterniond::Identity();
node_data->high_resolution_point_cloud.push_back( node_data->high_resolution_point_cloud.push_back(
@ -80,23 +79,20 @@ TEST_F(ConstraintBuilder3DTest, FindsConstraints) {
{Eigen::Vector3f(0.1, 0.2, 0.3)}); {Eigen::Vector3f(0.1, 0.2, 0.3)});
node_data->rotational_scan_matcher_histogram = Eigen::VectorXf::Zero(3); node_data->rotational_scan_matcher_histogram = Eigen::VectorXf::Zero(3);
node_data->local_pose = transform::Rigid3d::Identity(); node_data->local_pose = transform::Rigid3d::Identity();
node.constant_data = node_data;
std::vector<TrajectoryNode> submap_nodes = {node};
SubmapId submap_id{0, 1}; SubmapId submap_id{0, 1};
Submap3D submap(0.1, 0.1, transform::Rigid3d::Identity()); Submap3D submap(0.1, 0.1, transform::Rigid3d::Identity(),
Eigen::VectorXf::Zero(3));
int expected_nodes = 0; int expected_nodes = 0;
for (int i = 0; i < 2; ++i) { for (int i = 0; i < 2; ++i) {
EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), expected_nodes); EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), expected_nodes);
for (int j = 0; j < 2; ++j) { for (int j = 0; j < 2; ++j) {
constraint_builder_->MaybeAddConstraint( constraint_builder_->MaybeAddConstraint(
submap_id, &submap, NodeId{0, 0}, node.constant_data.get(), submap_id, &submap, NodeId{0, 0}, node_data.get(),
submap_nodes, transform::Rigid3d::Identity(), transform::Rigid3d::Identity(), transform::Rigid3d::Identity());
transform::Rigid3d::Identity());
} }
constraint_builder_->MaybeAddGlobalConstraint( constraint_builder_->MaybeAddGlobalConstraint(
submap_id, &submap, NodeId{0, 0}, node.constant_data.get(), submap_id, &submap, NodeId{0, 0}, node_data.get(),
submap_nodes, Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity());
Eigen::Quaterniond::Identity());
constraint_builder_->NotifyEndOfNode(); constraint_builder_->NotifyEndOfNode();
thread_pool_.WaitUntilIdle(); thread_pool_.WaitUntilIdle();
EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), ++expected_nodes); EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), ++expected_nodes);